/*!
Compute the image addition: \f$ Ires = I1 - I2 \f$.
\param I1 : The first image.
\param I2 : The second image.
\param Ires : \f$ Ires = I1 - I2 \f$
\param saturate : If true, saturate the result to [0 ; 255] using vpMath::saturate, otherwise overflow may occur.
*/
void
vpImageTools::imageSubtract(const vpImage<unsigned char> &I1,
const vpImage<unsigned char> &I2,
vpImage<unsigned char> &Ires,
const bool saturate)
{
if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
throw (vpException(vpException::dimensionError, "The two images do not have the same size"));
}
if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) {
Ires.resize(I1.getHeight(), I1.getWidth());
}
unsigned char *ptr_I1 = I1.bitmap;
unsigned char *ptr_I2 = I2.bitmap;
unsigned char *ptr_Ires = Ires.bitmap;
unsigned int cpt = 0;
#if VISP_HAVE_SSE2
if (Ires.getSize() >= 16) {
for (; cpt <= Ires.getSize() - 16 ; cpt += 16, ptr_I1 += 16, ptr_I2 += 16, ptr_Ires += 16) {
const __m128i v1 = _mm_loadu_si128( (const __m128i*) ptr_I1);
const __m128i v2 = _mm_loadu_si128( (const __m128i*) ptr_I2);
const __m128i vres = saturate ? _mm_subs_epu8(v1, v2) : _mm_sub_epi8(v1, v2);
_mm_storeu_si128( (__m128i*) ptr_Ires, vres );
}
}
#endif
for (; cpt < Ires.getSize(); cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) {
*ptr_Ires = saturate ? vpMath::saturate<unsigned char>( (short int) *ptr_I1 - (short int) *ptr_I2 ) : *ptr_I1 - *ptr_I2;
}
}
/*!
Grabs a grayscale image from the selected camera. If the camera color
coding differs from vp1394CMUGrabber::MONO8, the acquired image is
converted in a gray level image to match the requested format.
\param I : Acquired gray level image.
*/
void
vp1394CMUGrabber::acquire(vpImage<unsigned char> &I)
{
// get image data
unsigned long length;
unsigned char *rawdata = NULL ;
int dropped;
unsigned int size;
open(I);
camera->AcquireImageEx(TRUE,&dropped);
rawdata = camera->GetRawData(&length);
size = I.getSize();
switch(_color) {
case vp1394CMUGrabber::MONO8:
memcpy(I.bitmap, (unsigned char *) rawdata, size);
break;
case vp1394CMUGrabber::MONO16:
vpImageConvert::MONO16ToGrey(rawdata, I.bitmap, size);
break;
case vp1394CMUGrabber::YUV411:
vpImageConvert::YUV411ToGrey(rawdata, I.bitmap, size);
break;
case vp1394CMUGrabber::YUV422:
vpImageConvert::YUV422ToGrey(rawdata, I.bitmap, size);
break;
case vp1394CMUGrabber::YUV444:
vpImageConvert::YUV444ToGrey(rawdata, I.bitmap, size);
break;
case vp1394CMUGrabber::RGB8:
vpImageConvert::RGBToGrey(rawdata, I.bitmap, size);
break;
default:
close();
vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
"Format conversion not implemented. "
"Acquisition failed.") );
break;
};
//unsigned short depth = 0;
//camera->GetVideoDataDepth(&depth);
//std::cout << "depth: " << depth << " computed: " << (float)(length/(I.getHeight() * I.getWidth())) << std::endl;
//memcpy(I.bitmap,rawdata,length);
}
/*!
\ingroup group_imgproc_contours
Draw the input contours on the color image.
\param I : Color image where we want to draw the input contours.
\param contours : Detected contours.
\param color : Drawing color.
*/
void vp::drawContours(vpImage<vpRGBa> &I, const std::vector<std::vector<vpImagePoint> > &contours, const vpColor &color) {
if (I.getSize() == 0) {
return;
}
for (std::vector<std::vector<vpImagePoint> >::const_iterator it1 = contours.begin(); it1 != contours.end(); ++it1) {
for (std::vector<vpImagePoint>::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) {
unsigned int i = (unsigned int) it2->get_i();
unsigned int j = (unsigned int) it2->get_j();
I[i][j] = vpRGBa(color.R, color.G, color.B);
}
}
}
/*!
\ingroup group_imgproc_contours
Draw the input contours on the binary image.
\param I : Grayscale image where we want to draw the input contours.
\param contours : Detected contours.
\param grayValue : Drawing grayscale color.
*/
void vp::drawContours(vpImage<unsigned char> &I, const std::vector<std::vector<vpImagePoint> > &contours, unsigned char grayValue) {
if (I.getSize() == 0) {
return;
}
for (std::vector<std::vector<vpImagePoint> >::const_iterator it1 = contours.begin(); it1 != contours.end(); ++it1) {
for (std::vector<vpImagePoint>::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) {
unsigned int i = (unsigned int) it2->get_i();
unsigned int j = (unsigned int) it2->get_j();
I[i][j] = grayValue;
}
}
}
/*!
\ingroup group_imgproc_sharpening
Sharpen a grayscale image using the unsharp mask technique.
\param I : The grayscale image to sharpen.
\param size : Size (must be odd) of the Gaussian blur kernel.
\param weight : Weight (between [0 - 1[) for the sharpening process.
*/
void vp::unsharpMask(vpImage<unsigned char> &I, const unsigned int size, const double weight) {
if(weight < 1.0 && weight >= 0.0) {
//Gaussian blurred image
vpImage<double> I_blurred;
vpImageFilter::gaussianBlur(I, I_blurred, size);
//Unsharp mask
for(unsigned int cpt = 0; cpt < I.getSize(); cpt++) {
double val = (I.bitmap[cpt] - weight*I_blurred.bitmap[cpt]) / (1 - weight);
I.bitmap[cpt] = vpMath::saturate<unsigned char>(val); //val > 255 ? 255 : (val < 0 ? 0 : val);
}
}
}
/*!
\ingroup group_imgproc_sharpening
Sharpen a color image using the unsharp mask technique.
\param I : The color image to sharpen.
\param size : Size (must be odd) of the Gaussian blur kernel.
\param weight : Weight (between [0 - 1[) for the sharpening process.
*/
void vp::unsharpMask(vpImage<vpRGBa> &I, const unsigned int size, const double weight) {
if(weight < 1.0 && weight >= 0.0) {
//Gaussian blurred image
vpImage<double> I_blurred_R, I_blurred_G, I_blurred_B;
vpImage<unsigned char> I_R, I_G, I_B;
vpImageConvert::split(I, &I_R, &I_G, &I_B);
vpImageFilter::gaussianBlur(I_R, I_blurred_R, size);
vpImageFilter::gaussianBlur(I_G, I_blurred_G, size);
vpImageFilter::gaussianBlur(I_B, I_blurred_B, size);
//Unsharp mask
for(unsigned int cpt = 0; cpt < I.getSize(); cpt++) {
double val_R = (I.bitmap[cpt].R - weight*I_blurred_R.bitmap[cpt]) / (1 - weight);
double val_G = (I.bitmap[cpt].G - weight*I_blurred_G.bitmap[cpt]) / (1 - weight);
double val_B = (I.bitmap[cpt].B - weight*I_blurred_B.bitmap[cpt]) / (1 - weight);
I.bitmap[cpt].R = vpMath::saturate<unsigned char>(val_R);
I.bitmap[cpt].G = vpMath::saturate<unsigned char>(val_G);
I.bitmap[cpt].B = vpMath::saturate<unsigned char>(val_B);
}
}
}
/*!
\ingroup group_imgproc_contours
Extract contours from a binary image.
\param I_original : Input binary image (0 means background, 1 means foreground, other values are not allowed).
\param contours : Detected contours.
\param contourPts : List of contours, each contour contains a list of contour points.
\param retrievalMode : Contour retrieval mode.
*/
void vp::findContours(const vpImage<unsigned char> &I_original, vpContour &contours, std::vector<std::vector<vpImagePoint> > &contourPts,
const vpContourRetrievalType& retrievalMode) {
if (I_original.getSize() == 0) {
return;
}
//Clear output results
contourPts.clear();
vpImage<int> I(I_original.getHeight(), I_original.getWidth());
for (unsigned int cpt = 0; cpt < I_original.getSize(); cpt++) {
I.bitmap[cpt] = I_original.bitmap[cpt];
}
int nbd = 1; //Newest border
int lnbd = 1; //Last newest border
//Background contour
//By default the root contour is a hole contour
vpContour *root = new vpContour(vp::CONTOUR_HOLE);
std::map<int, vpContour*> borderMap;
borderMap[lnbd] = root;
for (unsigned int i = 0; i < I.getHeight(); i++) {
lnbd = 1; //Reset LNBD at the beginning of each scan row
for (unsigned int j = 0; j < I.getWidth(); j++) {
int fji = I[i][j];
bool isOuter = isOuterBorderStart(I, i, j);
bool isHole = isHoleBorderStart(I, i, j);
if (isOuter || isHole) { //else (1) (c)
vpContour *border = new vpContour;
vpContour *borderPrime = NULL;
vpImagePoint from(i, j);
if (isOuter) {
//(1) (a)
nbd++;
from.set_j(from.get_j() - 1);
border->m_contourType = vp::CONTOUR_OUTER;
borderPrime = borderMap[lnbd];
//Table 1
switch (borderPrime->m_contourType) {
case vp::CONTOUR_OUTER:
border->setParent(borderPrime->m_parent);
break;
case vp::CONTOUR_HOLE:
border->setParent(borderPrime);
break;
default:
break;
}
} else {
//(1) (b)
nbd++;
if (fji > 1) {
lnbd = fji;
}
borderPrime = borderMap[lnbd];
from.set_j(from.get_j() + 1);
border->m_contourType = vp::CONTOUR_HOLE;
//Table 1
switch (borderPrime->m_contourType) {
case vp::CONTOUR_OUTER:
border->setParent(borderPrime);
break;
case vp::CONTOUR_HOLE:
border->setParent(borderPrime->m_parent);
break;
default:
break;
}
}
vpImagePoint ij(i, j);
followBorder(I, ij, from, border, nbd);
//(3) (1) ; single pixel contour
if (border->m_points.empty()) {
border->m_points.push_back(ij);
I[i][j] = -nbd;
}
//Compute contour polygon
border->m_contourPolygon.buildFrom(border->m_points);
if (retrievalMode == CONTOUR_RETR_LIST || retrievalMode == CONTOUR_RETR_TREE) {
//Add contour points
contourPts.push_back(border->m_points);
}
borderMap[nbd] = border;
}
//(4)
if (fji != 0 && fji != 1) {
lnbd = std::abs(fji);
}
}
}
if (retrievalMode == CONTOUR_RETR_EXTERNAL || retrievalMode == CONTOUR_RETR_LIST) {
//Delete contours content
contours.m_parent = NULL;
for (std::vector<vpContour *>::iterator it = contours.m_children.begin(); it != contours.m_children.end(); ++it) {
(*it)->m_parent = NULL;
if (*it != NULL) {
delete *it;
*it = NULL;
}
}
contours.m_children.clear();
}
if (retrievalMode == CONTOUR_RETR_EXTERNAL) {
//Add only external contours
for (std::vector<vpContour*>::const_iterator it = root->m_children.begin(); it != root->m_children.end(); ++it) {
contours.m_children.push_back(new vpContour(**it));
contourPts.push_back((*it)->m_points);
}
} else if (retrievalMode == CONTOUR_RETR_LIST) {
getContoursList(*root, 0, contours);
//Set parent to root
for (std::vector<vpContour*>::iterator it = contours.m_children.begin(); it != contours.m_children.end(); ++it) {
(*it)->m_parent = &contours;
}
} else {
//CONTOUR_RETR_TREE
contours = *root;
}
delete root;
root = NULL;
}
void MSRCR(vpImage<vpRGBa> &I, const int _scale, const int scaleDiv,
const int level, const double dynamic, const int _kernelSize) {
//Calculate the scales of filtering according to the number of filter and their distribution.
std::vector<double> retinexScales = retinexScalesDistribution(scaleDiv, level, _scale);
//Filtering according to the various scales.
//Summarize the results of the various filters according to a specific weight(here equivalent for all).
double weight = 1.0 / (double) scaleDiv;
std::vector<vpImage<double> > doubleRGB(3);
std::vector<vpImage<double> > doubleResRGB(3);
unsigned int size = I.getSize();
int kernelSize = _kernelSize;
if(kernelSize == -1) {
//Compute the kernel size from the input image size
kernelSize = (int) (std::min(I.getWidth(), I.getHeight()) / 2.0);
kernelSize = (kernelSize - kernelSize%2) + 1;
}
for(int channel = 0; channel < 3; channel++) {
doubleRGB[(size_t) channel] = vpImage<double>(I.getHeight(), I.getWidth());
doubleResRGB[(size_t) channel] = vpImage<double>(I.getHeight(), I.getWidth());
for(unsigned int cpt = 0; cpt < size; cpt++) {
//Shift the pixel values by 1 to avoid problem with log(0)
switch(channel) {
case 0:
doubleRGB[(size_t) channel].bitmap[cpt] = I.bitmap[cpt].R + 1.0;
break;
case 1:
doubleRGB[(size_t) channel].bitmap[cpt] = I.bitmap[cpt].G + 1.0;
break;
case 2:
doubleRGB[(size_t) channel].bitmap[cpt] = I.bitmap[cpt].B + 1.0;
break;
default:
break;
}
}
for (int sc = 0; sc < scaleDiv; sc++) {
vpImage<double> blurImage;
double sigma = retinexScales[(size_t) sc];
vpImageFilter::gaussianBlur(doubleRGB[(size_t) channel], blurImage, (unsigned int) kernelSize, sigma);
for(unsigned int cpt = 0; cpt < size; cpt++) {
//Summarize the filtered values.
//In fact one calculates a ratio between the original values and the filtered values.
doubleResRGB[(size_t) channel].bitmap[cpt] += weight * (std::log(doubleRGB[(size_t) channel].bitmap[cpt])
- std::log(blurImage.bitmap[cpt]));
}
}
}
std::vector<double> dest(size*3);
const double gain = 1.0, alpha = 128.0, offset = 0.0;
for(unsigned int cpt = 0; cpt < size; cpt++) {
double logl = std::log( (double) (I.bitmap[cpt].R + I.bitmap[cpt].G + I.bitmap[cpt].B + 3.0) );
dest[cpt*3] = gain * (std::log(alpha * doubleRGB[0].bitmap[cpt]) - logl) * doubleResRGB[0].bitmap[cpt] + offset;
dest[cpt*3 + 1] = gain * (std::log(alpha * doubleRGB[1].bitmap[cpt]) - logl) * doubleResRGB[1].bitmap[cpt] + offset;
dest[cpt*3 + 2] = gain * (std::log(alpha * doubleRGB[2].bitmap[cpt]) - logl) * doubleResRGB[2].bitmap[cpt] + offset;
}
double sum = std::accumulate(dest.begin(), dest.end(), 0.0);
double mean = sum / dest.size();
std::vector<double> diff(dest.size());
std::transform(dest.begin(), dest.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
double stdev = std::sqrt(sq_sum / dest.size());
double mini = mean - dynamic*stdev;
double maxi = mean + dynamic*stdev;
double range = maxi - mini;
if(vpMath::nul(range)) {
range = 1.0;
}
for(unsigned int cpt = 0; cpt < size; cpt++) {
I.bitmap[cpt].R = vpMath::saturate<unsigned char>((255.0 * (dest[cpt*3 + 0] - mini) / range));
I.bitmap[cpt].G = vpMath::saturate<unsigned char>((255.0 * (dest[cpt*3 + 1] - mini) / range));
I.bitmap[cpt].B = vpMath::saturate<unsigned char>((255.0 * (dest[cpt*3 + 2] - mini) / range));
}
}
