/*IMAGEGRID */
void ImageGrid::populate(){
//init vectors
elements.resize(rows);
for(int row = 0; row<rows; row++){
elements[row].resize(cols);
}
cv::Mat mat = image.getMat();
int elementWidth = static_cast<int>(ceil((double)mat.cols/cols));
int elementHeight = static_cast<int>(ceil((double)mat.rows/rows));
int count = 0; //debugging
cv::Rect window = cv::Rect(0, 0, elementWidth, elementHeight);
for(int row = 0; row<rows-1; row++){
for(int col = 0; col<cols-1; col++){
cv::Mat ROI(mat,window);
GridElement element(ROI, window);
elements[row][col] = element;
window.x = window.x + window.width;
count++;
}
window.x = 0;
window.y += window.height;
}
window.x = window.width*(cols-1);
//final row/column might have different dimensions
int remainingX = mat.cols - window.x;
int remainingY = mat.rows - window.y;
//final column except for last element, this we will handle last, can have a different width
window.width = remainingX;
window.y = 0;
for(int row = 0; row<rows-1; row++){
cv::Mat ROI(mat,window);
GridElement element(ROI, window);
elements[row][cols-1] = element;
window.y += window.height;
count++;
}
//final row except for last element, this we will handle last, can have a different height
window.width = elementWidth;
window.height = remainingY;
window.x = 0;
for(int col = 0; col<cols-1; col++){
cv::Mat ROI(mat,window);
GridElement element(ROI, window);
elements[rows-1][col] = element;
window.x += window.width;
count++;
}
//final element can have a different width and height
window.width = remainingX;
cv::Mat ROI(mat,window);
GridElement element(ROI, window);
elements[rows-1][cols-1] = element;
count++;
}
int main(int argc, char ** argv){
std::string inputFileName;
if (argc>1)
inputFileName = argv[1];
else
return 0;
cv::Mat image = cv::imread(inputFileName);
cv::Mat imageHSV;
if(image.empty()){
std::cout << "Couldn't open " << inputFileName << std::endl;
return -1;
}
cv::cvtColor(image, imageHSV, CV_BGR2HSV);
// create window and UI
cv::namedWindow(ORIG_NAME, WINDOW_FLAGS);
cv::namedWindow(FG_NAME, WINDOW_FLAGS);
ROI rect = ROI(image);
// event handlers
//int mouseParam = CV_EVENT_FLAG_LBUTTON;
cv::setMouseCallback(ORIG_NAME,mouseHandler, &rect);
cv::imshow(ORIG_NAME, image);
}
Calc::Calc(const Config& config) :
_scale(config["calc.scale"].as<double>()),
_nx(config["output.width"].as<size_t>()),
_ny(config["output.height"].as<size_t>()),
_width(config["crop.width"].as<size_t>()),
_height(config["crop.height"].as<size_t>())
{
const size_t window_size = config["calc.window_size"].as<size_t>() | 1;
_window_size = cv::Size(window_size, window_size);
// Init background image
cv::Rect ROI(config["crop.xmin"].as<size_t>(), config["crop.ymin"].as<size_t>(),
config["crop.width"].as<size_t>(), config["crop.height"].as<size_t>());
if (config.count("input.background"))
{
_background = cv::imread(config["input.background"].as<std::string>(),
CV_LOAD_IMAGE_GRAYSCALE);
if (_background.size() != ROI.size())
{
_background = _background(ROI);
}
}
else
{
_background = cv::Mat::ones(ROI.size(), CV_8UC1) * 255;
}
// Init matrices
const int dims[] = { _ny, _nx, 2 };
_out.density.create(_height, _width, CV_8U);
_out.velocity.create(3, dims, CV_32F);
_out.mask.create(2, dims, CV_8U);
}
ROI
roi_intersection (const ROI &A, const ROI &B)
{
return ROI (std::max (A.xbegin, B.xbegin), std::min (A.xend, B.xend),
std::max (A.ybegin, B.ybegin), std::min (A.yend, B.yend),
std::max (A.zbegin, B.zbegin), std::min (A.zend, B.zend),
std::max (A.chbegin, B.chbegin), std::min (A.chend, B.chend));
}
// Tests histogram computation.
void histogram_computation_test ()
{
const int INPUT_WIDTH = 64;
const int INPUT_HEIGHT = 64;
const int INPUT_CHANNEL = 0;
const int HISTOGRAM_BINS = 256;
const int SPIKE1 = 51; // 0.2f in range 0->1 maps to 51 in range 0->255
const int SPIKE2 = 128; // 0.5f in range 0->1 maps to 128 in range 0->255
const int SPIKE3 = 204; // 0.8f in range 0->1 maps to 204 in range 0->255
const int SPIKE1_COUNT = INPUT_WIDTH * 8;
const int SPIKE2_COUNT = INPUT_WIDTH * 16;
const int SPIKE3_COUNT = INPUT_WIDTH * 40;
// Create input image with three regions with different pixel values.
ImageSpec spec (INPUT_WIDTH, INPUT_HEIGHT, 1, TypeDesc::FLOAT);
ImageBuf A (spec);
float value[] = {0.2f};
ImageBufAlgo::fill (A, value, ROI(0, INPUT_WIDTH, 0, 8));
value[0] = 0.5f;
ImageBufAlgo::fill (A, value, ROI(0, INPUT_WIDTH, 8, 24));
value[0] = 0.8f;
ImageBufAlgo::fill (A, value, ROI(0, INPUT_WIDTH, 24, 64));
// Compute A's histogram.
std::vector<imagesize_t> hist;
ImageBufAlgo::histogram (A, INPUT_CHANNEL, hist, HISTOGRAM_BINS);
// Does the histogram size equal the number of bins?
OIIO_CHECK_EQUAL (hist.size(), (imagesize_t)HISTOGRAM_BINS);
// Are the histogram values as expected?
OIIO_CHECK_EQUAL (hist[SPIKE1], (imagesize_t)SPIKE1_COUNT);
OIIO_CHECK_EQUAL (hist[SPIKE2], (imagesize_t)SPIKE2_COUNT);
OIIO_CHECK_EQUAL (hist[SPIKE3], (imagesize_t)SPIKE3_COUNT);
for (int i = 0; i < HISTOGRAM_BINS; i++)
if (i!=SPIKE1 && i!=SPIKE2 && i!=SPIKE3)
OIIO_CHECK_EQUAL (hist[i], 0);
}
TEST_P(DisparityWLSFilterTest, MultiThreadReproducibility)
{
if (cv::getNumberOfCPUs() == 1)
return;
double MAX_DIF = 1.0;
double MAX_MEAN_DIF = 1.0 / 256.0;
int loopsCount = 2;
RNG rng(0);
DisparityWLSParams params = GetParam();
Size size = get<0>(params);
int srcType = get<1>(params);
int guideType = get<2>(params);
bool use_conf = get<3>(params);
bool use_downscale = get<4>(params);
Mat left(size, guideType);
randu(left, 0, 255);
Mat left_disp(size,srcType);
int max_disp = (int)(size.width*0.1);
randu(left_disp, 0, max_disp-1);
Mat right_disp(size,srcType);
randu(left_disp, -max_disp+1, 0);
Rect ROI(max_disp,0,size.width-max_disp,size.height);
if(use_downscale)
{
resize(left_disp,left_disp,Size(),0.5,0.5);
resize(right_disp,right_disp,Size(),0.5,0.5);
ROI = Rect(ROI.x/2,ROI.y/2,ROI.width/2,ROI.height/2);
}
for (int iter = 0; iter <= loopsCount; iter++)
{
double lambda = rng.uniform(100.0, 10000.0);
double sigma = rng.uniform(1.0, 100.0);
Ptr<DisparityWLSFilter> wls_filter = createDisparityWLSFilterGeneric(use_conf);
wls_filter->setLambda(lambda);
wls_filter->setSigmaColor(sigma);
cv::setNumThreads(cv::getNumberOfCPUs());
Mat resMultiThread;
wls_filter->filter(left_disp,left,resMultiThread,right_disp,ROI);
cv::setNumThreads(1);
Mat resSingleThread;
wls_filter->filter(left_disp,left,resSingleThread,right_disp,ROI);
EXPECT_LE(cv::norm(resSingleThread, resMultiThread, NORM_INF), MAX_DIF);
EXPECT_LE(cv::norm(resSingleThread, resMultiThread, NORM_L1), MAX_MEAN_DIF*left.total());
}
}
void iterator_wrap_test (ImageBuf::WrapMode wrap, std::string wrapname)
{
const int WIDTH = 4, HEIGHT = 4, CHANNELS = 3;
static float buf[HEIGHT][WIDTH][CHANNELS] = {
{ {0,0,0}, {1,0,1}, {2,0,2}, {3,0,3} },
{ {0,1,4}, {1,1,5}, {2,1,6}, {3,1,7} },
{ {0,2,8}, {1,2,9}, {2,2,10}, {3,2,11} },
{ {0,3,12}, {1,3,13}, {2,3,14}, {3,3,15} }
};
ImageSpec spec (WIDTH, HEIGHT, CHANNELS, TypeDesc::FLOAT);
ImageBuf A (spec, buf);
std::cout << "iterator_wrap_test " << wrapname << ":";
int i = 0;
int noutside = 0;
for (ITERATOR p (A, ROI(-2, WIDTH+2, -2, HEIGHT+2), wrap);
!p.done(); ++p, ++i) {
if ((i % 8) == 0)
std::cout << "\n ";
std::cout << " " << p[0] << ' ' << p[1] << ' ' << p[2];
// Check wraps
if (! p.exists()) {
++noutside;
if (wrap == ImageBuf::WrapBlack) {
OIIO_CHECK_EQUAL (p[0], 0.0f);
OIIO_CHECK_EQUAL (p[1], 0.0f);
OIIO_CHECK_EQUAL (p[2], 0.0f);
} else if (wrap == ImageBuf::WrapClamp) {
ITERATOR q = p;
q.pos (clamp (p.x(), 0, WIDTH-1), clamp (p.y(), 0, HEIGHT-1));
OIIO_CHECK_EQUAL (p[0], q[0]);
OIIO_CHECK_EQUAL (p[1], q[1]);
OIIO_CHECK_EQUAL (p[2], q[2]);
} else if (wrap == ImageBuf::WrapPeriodic) {
ITERATOR q = p;
q.pos (p.x() % WIDTH, p.y() % HEIGHT);
OIIO_CHECK_EQUAL (p[0], q[0]);
OIIO_CHECK_EQUAL (p[1], q[1]);
OIIO_CHECK_EQUAL (p[2], q[2]);
} else if (wrap == ImageBuf::WrapMirror) {
ITERATOR q = p;
int x = p.x(), y = p.y();
wrap_mirror (x, 0, WIDTH);
wrap_mirror (y, 0, HEIGHT);
q.pos (x, y);
OIIO_CHECK_EQUAL (p[0], q[0]);
OIIO_CHECK_EQUAL (p[1], q[1]);
OIIO_CHECK_EQUAL (p[2], q[2]);
}
}
}
std::cout << "\n";
OIIO_CHECK_EQUAL (noutside, 48); // Should be 48 wrapped pixels
}
/**
Creates a ROI in the center of the last image
*/
void ROI_BG::segment()
{
width = 200; //width of ROI
height = ptrData->getLastImage()->rows; //height of ROI
x = ptrData->getLastImage()->cols / 2;
y = ptrData->getLastImage()->rows / 2;
Mat ROI(*ptrData->getLastImage(), Rect(x - width / 2, y - height / 2, width, height));
*out = ROI;
ptrData->addImage(out);
}
void CaptureThreadC::CaptureCamera() {
bool colorFrameAdded = false;
cv::VideoCapture capture(0);
//keep getting last frame until told to stop
int frame_count = 0;
while (isRunning) {
//time stuff
struct timeb start;
ftime(&start);
capture.set(CV_CAP_PROP_FRAME_WIDTH, 320);
capture.set(CV_CAP_PROP_FRAME_HEIGHT, 240);
int frameheight = capture.get(CV_CAP_PROP_FRAME_HEIGHT);
int framewidth = capture.get(CV_CAP_PROP_FRAME_WIDTH);
cv::Mat bufferMat(frameheight, framewidth, CV_8UC3);
capture >> bufferMat;
cv::resize(bufferMat, bufferMat, CvSize(1200, 600));
framesBuffer.enqueue(bufferMat);
colorFrameAdded = true;
if (colorFrameAdded == true)
{
cv::Mat currentFrame = bufferMat.clone();
cv::Mat gray_img;
vector<cv::Point> points; //to hold this frame face points
int topLeftx = framewidth / 8;
int topLefty = frameheight / 3;
int bottomRightx = framewidth * 2;
int bottomRighty = frameheight;
samplingPoints.enqueue(points);
cv::Rect ROI(cv::Point(topLeftx, topLefty), cv::Point(bottomRightx, bottomRighty));
faceArea.enqueue(ROI);
}
}
colorFrameAdded = false;
//regulate fps
struct timeb end;
ftime(&end);
}
int ManualLabeling::growFromSeedRaw(const cv::Mat& rawImage, cv::Mat&labeledImage, cv::Point2i seedPoint) {
//assume that the labeled Mat is allocated.
cv::Mat segmentedImage;
int rectOff = 100;
int rectSide = rectOff*2+1;
//The model of the image is that is a orange blob.
cv::Rect ROI(seedPoint.x - rectOff, seedPoint.y - rectOff, rectSide, rectSide);
catheterFloodFillSegmentation(rawImage, segmentedImage, seedPoint, ROI);
//catheterImageSegmentation(rawImage, segmentedImage,seedPt,ROI);
return growFromSeed(segmentedImage, labeledImage, seedPoint);
}
void AllignedFrameSource::nextFrame(cv::OutputArray frame)
{
base_->nextFrame(origFrame_);
if (origFrame_.rows % scale_ == 0 && origFrame_.cols % scale_ == 0)
{
cv::superres::arrCopy(origFrame_, frame);
}
else
{
cv::Rect ROI(0, 0, (origFrame_.cols / scale_) * scale_, (origFrame_.rows / scale_) * scale_);
cv::superres::arrCopy(origFrame_(ROI), frame);
}
}
CImage *CROI(CImage *cimg, int xl, int yl, int xr, int yr)
{
CImage *croi=NULL;
int i;
if (ValidPixel(cimg->C[0],xl,yl)&&ValidPixel(cimg->C[0],xr,yr)&&
(xl <= xr)&&(yl <= yr)) {
croi = (CImage *) calloc(1,sizeof(CImage));
if (croi == NULL){
Error(MSG1,"CreateCImage");
}
for (i=0; i<3; i++)
croi->C[i] = ROI(cimg->C[i], xl, yl, xr, yr);
}
return (croi);
}
static bool
fill_ (ImageBuf &dst, const float *values, ROI roi=ROI(), int nthreads=1)
{
if (nthreads != 1 && roi.npixels() >= 1000) {
// Lots of pixels and request for multi threads? Parallelize.
ImageBufAlgo::parallel_image (
boost::bind(fill_<T>, boost::ref(dst), values,
_1 /*roi*/, 1 /*nthreads*/),
roi, nthreads);
return true;
}
// Serial case
for (ImageBuf::Iterator<T> p (dst, roi); !p.done(); ++p)
for (int c = roi.chbegin; c < roi.chend; ++c)
p[c] = values[c];
return true;
}
void LBP::Procura(Mat &Query)
{
Mat ROI(Size(WIDTH,HEIGHT),CV_32FC1, Scalar::all(0));
Mat LBP;
Point roi; // Armazena as coordenadas das Features
int raio=1; int vizinhaca=8;
DetectFace df;
faces.clear();
IMAGENSPOSITIVAS=0;IMAGENSNEGATIVAS=0;
// convoluçao para gerar uma imagem de 320 x 240 px em NAO faces de 25 x 30 px
for(int i = 0; i <= Query.rows - HEIGHT ; i++)
{
roi.y = i;
for(int j =0; j <= Query.cols - WIDTH ; j++)
{
roi.x = j;
Query.operator ()(Rect(roi.x,roi.y,WIDTH,HEIGHT)).convertTo(ROI,CV_32FC1,1,0);
PadraoLocal(ROI,LBP,raio,vizinhaca);
Mat temp;
MatConstIterator_<float> it = LBP.begin<float>(), it_end = LBP.end<float>();
for(; it != it_end; ++it) temp.push_back(*it);
PREDICAO = boost.predict( temp, Mat(),Range::all(),false,true);
//Bboost.predict( const float* row_sample, int row_len, bool returnDFVal=false ) const;
//svm.predict()
QString nome = QString("ROI00%1-%2.jpg").arg(i).arg(j);
string result2 = nome.toUtf8().constData();
if ( PREDICAO > 12 )
{
df.predicao = PREDICAO; df.ponto = roi;
faces.push_back( df );
IMAGENSPOSITIVAS++;
}
else
IMAGENSNEGATIVAS++;
}
}
}
void test_paste ()
{
std::cout << "test paste\n";
// Create the source image, make it a gradient
ImageSpec Aspec (4, 4, 3, TypeDesc::FLOAT);
ImageBuf A ("A", Aspec);
for (ImageBuf::Iterator<float> it (A); !it.done(); ++it) {
it[0] = float(it.x()) / float(Aspec.width-1);
it[1] = float(it.y()) / float(Aspec.height-1);
it[2] = 0.1f;
}
// Create destination image -- black it out
ImageSpec Bspec (8, 8, 3, TypeDesc::FLOAT);
ImageBuf B ("B", Bspec);
float gray[3] = { .1, .1, .1 };
ImageBufAlgo::fill (B, gray);
// Paste a few pixels from A into B -- include offsets
ImageBufAlgo::paste (B, 2, 2, 0, 1 /* chan offset */,
A, ROI(1, 4, 1, 4));
// Spot check
float a[3], b[3];
B.getpixel (1, 1, 0, b);
OIIO_CHECK_EQUAL (b[0], gray[0]);
OIIO_CHECK_EQUAL (b[1], gray[1]);
OIIO_CHECK_EQUAL (b[2], gray[2]);
B.getpixel (2, 2, 0, b);
A.getpixel (1, 1, 0, a);
OIIO_CHECK_EQUAL (b[0], gray[0]);
OIIO_CHECK_EQUAL (b[1], a[0]);
OIIO_CHECK_EQUAL (b[2], a[1]);
B.getpixel (3, 4, 0, b);
A.getpixel (2, 3, 0, a);
OIIO_CHECK_EQUAL (b[0], gray[0]);
OIIO_CHECK_EQUAL (b[1], a[0]);
OIIO_CHECK_EQUAL (b[2], a[1]);
}
void CameraConfigure()
{
CloseDisplay();
int w = 600, h= 600;
fastCMOS->setFramerate(10);
int fps= fastCMOS->getFramerate().fps;
ROI rfit = fastCMOS->setROI(ROI(w,h));
ROI roi = fastCMOS->getROI();
fastCMOS->setup(50);
fastCMOS->start();
//IMAQBuffer* buf = fastCMOS->snap();
int mode = fastCMOS->getMode();
InitDisplay(w,h);
// IMAQBuffer* buf = fastCMOS->snap();
// void *buf = fastCMOS->getLastFrame();
}
std::list<ROI> *
GradientClassifier::classify()
{
if (_q->get_buffer() == NULL) {
//cout << "GradientClassifier: ERROR, src buffer not set. NOT classifying." << endl;
return new std::list<ROI>;
}
list<ROI> *rv = new list<ROI>;
int cur_val, cur_diff, direction = 0;
upoint_t cur_pos, edge_start;
cur_pos.x = cur_pos.y = edge_start.x = edge_start.y = 0;
unsigned int jumpSize = 0;
ROI current;
for (list<ScanlineGrid *>::iterator it = _scanlines->begin(); it != _scanlines->end(); it++) {
ScanlineGrid *slm = (*it);
slm->reset();
_last_pos = *(*slm);
_last_val = _q->get(_last_pos);
while (!slm->finished()) {
cur_pos = *(++(*slm));
cur_val = _q->get(cur_pos);
cur_diff = cur_val - _last_val;
if ((cur_pos.x < _last_pos.x || cur_pos.y < _last_pos.y) //new scan line
|| (current.pixel_step
&& ((cur_pos.x - current.start.x) > _max_size //area found is too big
|| (cur_pos.y - current.start.y) > _max_size))) {
current.set_pixel_step(0);
edge_start.x = edge_start.y = direction = jumpSize = 0;
}
int curDir = (cur_diff < 0 ? -1 : (cur_diff > 0 ? 1 : 0));
switch (curDir) {
case -1:
switch (direction) {
case -1: //drop continues
jumpSize -= cur_diff;
break;
case 0: //new drop
jumpSize = -cur_diff;
edge_start = cur_pos;
break;
case 1:
if (jumpSize < _threshold) //spike reset ramp
{
jumpSize = -cur_diff;
edge_start = cur_pos;
} else // found edge!
{
if (current.pixel_step) //this is a line end
{
current.set_width(_last_pos.x - current.start.x);
current.set_height(_last_pos.y - current.start.y);
rv->push_back(ROI(current));
current.set_pixel_step(0);
} else if (_use_falling_edge) {
current.set_pixel_step(1);
current.set_start(edge_start);
}
edge_start = cur_pos;
jumpSize = -cur_diff;
}
break;
}
direction = -1;
break;
case 0:
switch (direction) {
case -1: //ramp end
case 1: //ramp end
if (jumpSize >= _threshold) //found edge!
{
if (current.pixel_step) //this is a line end
{
current.set_width(_last_pos.x - current.start.x);
current.set_height(_last_pos.y - current.start.y);
rv->push_back(ROI(current));
current.set_pixel_step(0);
} else {
if ((_use_falling_edge && direction == 1) || (_use_rising_edge && direction == -1)) {
current.set_pixel_step(1);
current.set_start(edge_start);
}
}
}
break;
case 0: break;
}
direction = jumpSize = 0;
edge_start.x = edge_start.y = 0;
break;
case 1:
switch (direction) {
case 1: //climb continues
jumpSize += cur_diff;
break;
case 0: //new climb
jumpSize = cur_diff;
edge_start = cur_pos;
break;
case -1:
if (jumpSize < _threshold) //spike reset ramp
{
jumpSize = cur_diff;
edge_start = cur_pos;
} else // found edge!
{
if (current.pixel_step) //this is a line end
{
current.set_width(_last_pos.x - current.start.x);
current.set_height(_last_pos.y - current.start.y);
rv->push_back(ROI(current));
current.set_pixel_step(0);
} else if (_use_rising_edge) {
current.set_pixel_step(1);
current.set_start(edge_start);
}
edge_start = cur_pos;
jumpSize = cur_diff;
}
break;
}
direction = 1;
break;
}
_last_val = cur_val;
_last_pos = cur_pos;
}
} //END: For all scanline models
return rv;
}
// Test ImageBuf::zero and ImageBuf::fill
void test_zero_fill ()
{
const int WIDTH = 8;
const int HEIGHT = 6;
const int CHANNELS = 4;
ImageSpec spec (WIDTH, HEIGHT, CHANNELS, TypeDesc::FLOAT);
spec.alpha_channel = 3;
// Create a buffer -- pixels should be undefined
ImageBuf A ("A", spec);
// Set a pixel to an odd value, make sure it takes
const float arbitrary1[CHANNELS] = { 0.2, 0.3, 0.4, 0.5 };
A.setpixel (1, 1, arbitrary1);
float pixel[CHANNELS]; // test pixel
A.getpixel (1, 1, pixel);
for (int c = 0; c < CHANNELS; ++c)
OIIO_CHECK_EQUAL (pixel[c], arbitrary1[c]);
// Zero out and test that it worked
ImageBufAlgo::zero (A);
for (int j = 0; j < HEIGHT; ++j) {
for (int i = 0; i < WIDTH; ++i) {
float pixel[CHANNELS];
A.getpixel (i, j, pixel);
for (int c = 0; c < CHANNELS; ++c)
OIIO_CHECK_EQUAL (pixel[c], 0.0f);
}
}
// Test fill of whole image
const float arbitrary2[CHANNELS] = { 0.6, 0.7, 0.3, 0.9 };
ImageBufAlgo::fill (A, arbitrary2);
for (int j = 0; j < HEIGHT; ++j) {
for (int i = 0; i < WIDTH; ++i) {
float pixel[CHANNELS];
A.getpixel (i, j, pixel);
for (int c = 0; c < CHANNELS; ++c)
OIIO_CHECK_EQUAL (pixel[c], arbitrary2[c]);
}
}
// Test fill of partial image
const float arbitrary3[CHANNELS] = { 0.42, 0.43, 0.44, 0.45 };
{
const int xbegin = 3, xend = 5, ybegin = 0, yend = 4;
ImageBufAlgo::fill (A, arbitrary3, ROI(xbegin, xend, ybegin, yend));
for (int j = 0; j < HEIGHT; ++j) {
for (int i = 0; i < WIDTH; ++i) {
float pixel[CHANNELS];
A.getpixel (i, j, pixel);
if (j >= ybegin && j < yend && i >= xbegin && i < xend) {
for (int c = 0; c < CHANNELS; ++c)
OIIO_CHECK_EQUAL (pixel[c], arbitrary3[c]);
} else {
for (int c = 0; c < CHANNELS; ++c)
OIIO_CHECK_EQUAL (pixel[c], arbitrary2[c]);
}
}
}
}
}
int main()
{
cv::Mat imCalibColor;
cv::Mat imCalibGray;
cv::vector<cv::vector<cv::Point> > contours;
cv::vector<cv::Vec4i> hierarchy;
cv::vector<cv::Point2f> pointQR;
cv::Mat imCalibNext;
cv::Mat imQR;
cv::vector<cv::Mat> tabQR;
/*cv::vector<cv::Point2f> corners1;
cv::vector<cv::Point2f> corners2;
cv::vector<cv::Point2f> corners3;
cv::vector<cv::Point2f> corners4;
cv::vector<cv::Point2f> corners5;*/
double qualityLevel = 0.01;
double minDistance = 10;
int blockSize = 3;
bool useHarrisDetector = false;
double k = 0.04;
int maxCorners = 600;
int A = 0, B= 0, C= 0;
char key;
int mark;
bool patternFound = false;
cv::VideoCapture vcap("../rsc/capture2.avi");
for (int i = 1; i < 5; i++)
{
std::ostringstream oss;
oss << "../rsc/QrCodes/QR" << i << ".jpg";
imQR = cv::imread(oss.str());
cv::cvtColor(imQR, imQR, CV_BGR2GRAY);
std::cout<< "Bouh!!!!!!" << std::endl;
tabQR.push_back(imQR);
}
do
{
while(imCalibColor.empty())
{
vcap >> imCalibColor;
}
vcap >> imCalibColor;
cv::Mat edges(imCalibColor.size(),CV_MAKETYPE(imCalibColor.depth(), 1));
cv::cvtColor(imCalibColor, imCalibGray, CV_BGR2GRAY);
Canny(imCalibGray, edges, 100 , 200, 3);
cv::findContours( edges, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
cv::imshow("pointInteret", imCalibColor);
mark = 0;
cv::vector<cv::Moments> mu(contours.size());
cv::vector<cv::Point2f> mc(contours.size());
for( int i = 0; i < contours.size(); i++ )
{
mu[i] = moments( contours[i], false );
mc[i] = cv::Point2f( mu[i].m10/mu[i].m00 , mu[i].m01/mu[i].m00 );
}
for( int i = 0; i < contours.size(); i++ )
{
int k=i;
int c=0;
while(hierarchy[k][2] != -1)
{
k = hierarchy[k][2] ;
c = c+1;
}
if(hierarchy[k][2] != -1)
c = c+1;
if (c >= 5)
{
if (mark == 0) A = i;
else if (mark == 1) B = i; // i.e., A is already found, assign current contour to B
else if (mark == 2) C = i; // i.e., A and B are already found, assign current contour to C
mark = mark + 1 ;
}
}
if (A !=0 && B !=0 && C!=0)
{
cv::Mat imagecropped = imCalibColor;
cv::Rect ROI(280/*pointQR[0].x*/, 260/*pointQR[0].y*/, 253, 218);
cv::Mat croppedRef(imagecropped, ROI);
cv::cvtColor(croppedRef, imagecropped, CV_BGR2GRAY);
cv::threshold(imagecropped, imagecropped, 180, 255, CV_THRESH_BINARY);
pointQR.push_back(mc[A]);
cv::circle(imCalibColor, cv::Point(pointQR[0].x, pointQR[0].y), 3, cv::Scalar(0, 0, 255), 1, 8, 0);
pointQR.push_back(mc[B]);
cv::circle(imCalibColor, cv::Point(pointQR[1].x, pointQR[1].y), 3, cv::Scalar(0, 0, 255), 1, 8, 0);
pointQR.push_back(mc[C]);
cv::circle(imCalibColor, cv::Point(pointQR[2].x, pointQR[2].y), 3, cv::Scalar(0, 0, 255), 1, 8, 0);
cv::Point2f D(0.0f,0.0f);
cv::Point2f E(0.0f,0.0f);
cv::Point2f F(0.0f,0.0f);
D.x = (mc[A].x + mc[B].x)/2;
E.x = (mc[B].x + mc[C].x)/2;
F.x = (mc[C].x + mc[A].x)/2;
D.y = (mc[A].y + mc[B].y)/2;
E.y = (mc[B].y + mc[C].y)/2;
F.y = (mc[C].y + mc[A].y)/2;
pointQR.push_back(D);
cv::circle(imCalibColor, cv::Point(pointQR[3].x, pointQR[3].y), 3, cv::Scalar(0, 0, 255), 1, 8, 0);
pointQR.push_back(E);
cv::circle(imCalibColor, cv::Point(pointQR[4].x, pointQR[4].y), 3, cv::Scalar(0, 0, 255), 1, 8, 0);
pointQR.push_back(F);
cv::circle(imCalibColor, cv::Point(pointQR[5].x, pointQR[5].y), 3, cv::Scalar(0, 0, 255), 1, 8, 0);
patternFound = true;
std::cout << "patternfound" << std::endl;
cv::SiftFeatureDetector detector;
cv::vector<cv::KeyPoint> keypoints1, keypoints2;
detector.detect(tabQR[3], keypoints1);
detector.detect(imagecropped, keypoints2);
cv::Ptr<cv::DescriptorExtractor> descriptor = cv::DescriptorExtractor::create("SIFT");
cv::Mat descriptors1, descriptors2;
descriptor->compute(tabQR[3], keypoints1, descriptors1 );
descriptor->compute(imagecropped, keypoints2, descriptors2 );
cv::FlannBasedMatcher matcher;
std::vector< cv::DMatch > matches;
matcher.match( descriptors1, descriptors2, matches );
double max_dist = 0; double min_dist = 100;
for( int i = 0; i < descriptors1.rows; i++ )
{
double dist = matches[i].distance;
if( dist < min_dist ) min_dist = dist;
if( dist > max_dist ) max_dist = dist;
}
std::vector< cv::DMatch > good_matches;
for( int i = 0; i < descriptors1.rows; i++ )
if( matches[i].distance <= 2*min_dist )
good_matches.push_back( matches[i]);
cv::Mat imgout;
drawMatches(tabQR[3], keypoints1, imagecropped, keypoints2, good_matches, imgout);
std::vector<cv::Point2f> pt_img1;
std::vector<cv::Point2f> pt_img2;
for( int i = 0; i < (int)good_matches.size(); i++ )
{
pt_img1.push_back(keypoints1[ good_matches[i].queryIdx ].pt );
pt_img2.push_back(keypoints2[ good_matches[i].trainIdx ].pt );
}
cv::Mat H = findHomography( pt_img1, pt_img2, CV_RANSAC );
cv::Mat result;
warpPerspective(tabQR[3],result,H,cv::Size(tabQR[3].cols+imagecropped.cols,tabQR[3].rows));
cv::Mat half(result,cv::Rect(0,0,imagecropped.cols,imagecropped.rows));
imagecropped.copyTo(half);
imshow( "Result", result );
break;
}
key = (char)cv::waitKey(67);
}while(patternFound != true && key != 27);
if(patternFound)
imCalibNext = imCalibColor;
return patternFound;
}
}
int FaceTracker::subRegionTrack(cv::Mat& _curFrame, std::vector<cv::Point2f>& _curFeatures, cv::Rect& faceRect, double& executionTime){
// do tracking in the subregion, we always have at least one history //
cv::Rect prevRect = faceTrajectroy.at(faceTrajectroy.size()-1);
int roix = (prevRect.x - prevRect.width) > 0 ? (prevRect.x - prevRect.width):0;
int roiy = (prevRect.y - prevRect.height) > 0 ? (prevRect.y - prevRect.height):0;
int roiW = (roix + (3 * prevRect.width)) <= _curFrame.cols? (3 * prevRect.width) : (_curFrame.cols - roix);
int roiH = (roiy + (3 * prevRect.height)) <= _curFrame.rows? (3 * prevRect.height) : (_curFrame.rows - roiy);
cv::Rect ROI( roix, roiy, roiW, roiH);
// crop original image, update feature points //
cv::Mat croppedPrevFrame(prevFrame, ROI);
cv::Mat croppedCurFrame(_curFrame, ROI);
std::vector<cv::Point2f> croppedPrevFeatures;
for (int i = 0 ; i < _prevFeatures.size(); ++i){
cv::Point2f p(_prevFeatures.at(i).x - ROI.x, _prevFeatures.at(i).y - ROI.y);
croppedPrevFeatures.push_back(p);
}
// Track on subregion
int returnValue;
cv::vector<float> err;
cv::vector<uchar> status;
std::vector<cv::Point2f> croppedCurFeatures;
cv::calcOpticalFlowPyrLK(croppedPrevFrame, croppedCurFrame, croppedPrevFeatures, croppedCurFeatures,
status, err, cv::Size(31,31), 3, termcrit, 0, 0.001);
cv::Point2f min_point(FLT_MAX, FLT_MAX);
cv::Point2f max_point(FLT_MIN, FLT_MIN);
// refactor the points array to remove points lost due to tracking error,
// and map it to the original image location
size_t i, k;
for (i = k = 0; i < croppedCurFeatures.size(); ++i){
// status[i] = 0, the feature has lost
//if (status[i] == 0){
// continue;
//}
if (status[i] == 0 || distance(croppedCurFeatures[i].x, croppedCurFeatures[i].y , croppedPrevFeatures[i].x , croppedPrevFeatures[i].y) > 30){
continue;
}
croppedCurFeatures[k].x = croppedCurFeatures[i].x + ROI.x;
croppedCurFeatures[k].y = croppedCurFeatures[i].y + ROI.y;
min_point.x = Min(min_point.x, croppedCurFeatures[k].x);
min_point.y = Min(min_point.y, croppedCurFeatures[k].y);
max_point.x = Max(max_point.x, croppedCurFeatures[k].x);
max_point.y = Max(max_point.y, croppedCurFeatures[k].y);
++k;
}
croppedCurFeatures.resize(k);
_curFeatures = croppedCurFeatures;
// stablize and decide the bounding box
faceRect.x = cvRound(min_point.x);
faceRect.y = cvRound(min_point.y);
faceRect.width = cvRound((max_point.x - min_point.x));
faceRect.height = cvRound((max_point.y - min_point.y));
fc.faceRect = faceRect;
fc.featurePoints = _curFeatures;
if (faceRect.width < 25 || faceRect.height < 25 || k < 15 || faceRect.height/(faceRect.width * 1.0) > 2.5){
return -1;
}
// estimate execution time //
executionTime = estimateExecutionTime(ROI, prevFrame);
// save into history //
faceTrajectroy.push_back(faceRect);
if (faceTrajectroy.size() == HISTORY_NUM + 1){
faceTrajectroy.pop_front();
}
// update //
_curFrame.copyTo(prevFrame);
_prevFeatures = _curFeatures;
}
int FaceTracker::track(cv::Mat& _curFrame, std::vector<cv::Point2f>& _curFeatures, cv::Rect& faceRect, double& executionTime){
// do tracking in the subregion, we always have at least one history //
cv::Rect prevRect = faceTrajectroy.at(faceTrajectroy.size()-1);
int roix = (prevRect.x - 1.5 * prevRect.width) > 0 ? (prevRect.x - 1.5 * prevRect.width):0;
int roiy = (prevRect.y - 1.5 * prevRect.height) > 0 ? (prevRect.y - 1.5 * prevRect.height):0;
int roiW = (roix + (4 * prevRect.width)) <= _curFrame.cols? (4 * prevRect.width) : (_curFrame.cols - roix);
int roiH = (roiy + (4 * prevRect.height)) <= _curFrame.rows? (4 * prevRect.height) : (_curFrame.rows - roiy);
cv::Rect ROI( roix, roiy, roiW, roiH);
//std::cout << "ROI" << ROI.x << "," << ROI.y << "," << ROI.width << "," << ROI.height <<std::endl;
/**
if (faceTrajectroy.size() > 1){
cv::Rect faceVelocity;
float alpha = 0.7;
for (int i = 1 ; i < faceTrajectroy.size(); ++i){
// estimate velocity
cv::Rect pRect = faceTrajectroy.at(i-1);
cv::Rect cRect = faceTrajectroy.at(i);
if (i == 1){
faceVelocity.x = cRect.x - pRect.x;
faceVelocity.y = cRect.y - pRect.y;
faceVelocity.width = cRect.width - pRect.width;
faceVelocity.height = cRect.height - pRect.height;
}else{
faceVelocity.x = alpha * (cRect.x - pRect.x) + (1-alpha) * faceVelocity.x;
faceVelocity.y = alpha * (cRect.y - pRect.y) + (1-alpha) * faceVelocity.y;
faceVelocity.width = alpha * (cRect.width - pRect.width) + (1-alpha) * faceVelocity.width;
faceVelocity.height = alpha * (cRect.height - pRect.height) + (1-alpha) * faceVelocity.height;
}
}
//update ROI
ROI.x = prevRect.x + faceVelocity.x;
ROI.y = prevRect.y + faceVelocity.y;
ROI.width = prevRect.width + faceVelocity.width;
ROI.height = prevRect.height + faceVelocity.height;
}
**/
// crop original image, update feature points //
cv::Mat croppedPrevFrame(prevFrame, ROI);
cv::Mat croppedCurFrame(_curFrame, ROI);
std::vector<cv::Point2f> croppedPrevFeatures;
for (int i = 0 ; i < _prevFeatures.size(); ++i){
cv::Point2f p(_prevFeatures.at(i).x - ROI.x, _prevFeatures.at(i).y - ROI.y);
croppedPrevFeatures.push_back(p);
}
/**
for (int i = 0; i < croppedPrevFeatures.size(); ++i){
circle( croppedPrevFrame, croppedPrevFeatures[i], 3, cv::Scalar(0,255,0), -1 , 8);
}
cv::imshow("croppedFrame", croppedPrevFrame);
cv::waitKey( 50 );
**/
// Track on subregion
int returnValue;
cv::vector<float> err;
cv::vector<uchar> status;
std::vector<cv::Point2f> croppedCurFeatures;
cv::calcOpticalFlowPyrLK(croppedPrevFrame, croppedCurFrame, croppedPrevFeatures, croppedCurFeatures,
status, err, cv::Size(31,31), 3, termcrit, 0, 0.001);
//cv::calcOpticalFlowPyrLK(prevFrame, _curFrame, _prevFeatures, _curFeatures,
// status, err, cv::Size(31,31), 3, termcrit, 0, 0.001);
cv::Point2f min_point(FLT_MAX, FLT_MAX);
cv::Point2f max_point(FLT_MIN, FLT_MIN);
// refactor the points array to remove points lost due to tracking error,
// and map it to the original image location
size_t i, k;
std::vector<cv::Point2f> velocity;
for (i = 0; i < croppedCurFeatures.size(); ++i){
// adjust it to the correct position
croppedCurFeatures.at(i).x += ROI.x;
croppedCurFeatures.at(i).y += ROI.y;
// status[i] = 0, the feature has lost
//if (status[i] == 0 || distance(croppedCurFeatures[i].x, croppedCurFeatures[i].y , _prevFeatures[i].x , _prevFeatures[i].y) > 30){
if (status[i] == 0){
continue;
}
// compute the moving speed for each of the good feature point //
cv::Point2f vp(croppedCurFeatures.at(i).x - _prevFeatures.at(i).x, croppedCurFeatures.at(i).y - _prevFeatures.at(i).y);
velocity.push_back(vp);
}
// tracker failed!
/**
if (velocity.size() < 3)
return -1;
std::sort(velocity.begin(), velocity.end(), myFunction);
int mid = velocity.size()/2;
int offset = velocity.size() % 2 == 0? mid: (mid+1);
float q1, q3 = 0;
if (mid % 2 == 0){
int q1_index = (mid/2 + 1) >= velocity.size()? (velocity.size()-1) :(mid/2 + 1);
q1 = (sqrt(velocity.at((mid/2)).x * velocity.at((mid/2)).x + velocity.at((mid/2)).y * velocity.at((mid/2)).y) +
sqrt(velocity.at(q1_index).x * velocity.at(q1_index).x + velocity.at(q1_index).y * velocity.at(q1_index).y))/2;
int q3_index_1 = (mid/2) + offset >= velocity.size()? velocity.size()-1:(mid/2) + offset;
int q3_index_2 = (mid/2)+ 1 + offset >= velocity.size()? velocity.size()-1:(mid/2) + offset +1 ;
q3 = (sqrt(velocity.at(q3_index_1).x * velocity.at(q3_index_1).x + velocity.at(q3_index_1).y * velocity.at(q3_index_1).y) +
sqrt(velocity.at(q3_index_2).x * velocity.at(q3_index_2).x + velocity.at(q3_index_2).y * velocity.at(q3_index_2).y))/2;
}else{
int q3_index = (mid/2) + offset >= velocity.size()? velocity.size()-1:(mid/2) + offset;
q1 = sqrt(velocity.at((mid/2)).x * velocity.at((mid/2)).x + velocity.at((mid/2)).y * velocity.at((mid/2)).y);
q3 = sqrt(velocity.at(q3_index).x * velocity.at(q3_index).x + velocity.at(q3_index).y * velocity.at(q3_index).y);
}
float interval = (q3 - q1) * 3;
**/
for(i = k = 0; i < croppedCurFeatures.size(); ++i){
// status[i] = 0, the feature has lost
if (status[i] == 0){
continue;
}
// for each working feature point //
/**
cv::Point2f vp(croppedCurFeatures.at(i).x - _prevFeatures.at(i).x, croppedCurFeatures.at(i).y - _prevFeatures.at(i).y);
float v = sqrt(vp.x * vp.x) + (vp.y * vp.y);
if (v > q3 + interval){
croppedCurFeatures.at(i).x = _prevFeatures.at(i).x + velocity.at(mid).x;
croppedCurFeatures.at(i).y = _prevFeatures.at(i).y + velocity.at(mid).y;
}
**/
croppedCurFeatures[k].x = croppedCurFeatures[i].x;
croppedCurFeatures[k].y = croppedCurFeatures[i].y;
min_point.x = Min(min_point.x, croppedCurFeatures[k].x);
min_point.y = Min(min_point.y, croppedCurFeatures[k].y);
max_point.x = Max(max_point.x, croppedCurFeatures[k].x);
max_point.y = Max(max_point.y, croppedCurFeatures[k].y);
++k;
}
croppedCurFeatures.resize(k);
_curFeatures = croppedCurFeatures;
// stablize and decide the bounding box
faceRect.x = cvRound(min_point.x);
faceRect.y = cvRound(min_point.y);
//faceRect.width = (cvRound((max_point.x - min_point.x)) + cvRound((max_point.y - min_point.y)))/2;
//faceRect.height = (cvRound((max_point.x - min_point.x)) + cvRound((max_point.y - min_point.y)))/2;
faceRect.width = cvRound(max_point.x - min_point.x);
faceRect.height = cvRound(max_point.y - min_point.y);
fc.faceRect = faceRect;
fc.featurePoints = _curFeatures;
// if (faceRect.width < 25 || faceRect.height < 25 || k < 15){
if (faceRect.width < 25 || faceRect.height < 25 || k < 3){
std::cout << "tracker fails because of width/height/k:" << faceRect.width << "," << faceRect.height << "," << k << std::endl;
return -1;
}
// estimate execution time //
executionTime = estimateExecutionTime(ROI, prevFrame);
// save into history //
faceTrajectroy.push_back(faceRect);
if (faceTrajectroy.size() == HISTORY_NUM + 1){
faceTrajectroy.pop_front();
}
// update //
_curFrame.copyTo(prevFrame);
_prevFeatures = _curFeatures;
int main(int argc, char **argv) {
if (argc < 3) {
std::cerr << "There are no enough parameters\n";
return -1;
}
cam_type_t cam_type = UNKNOWN_CAM;
if (strcmp("ip", argv[2]) == 0) {
std::cout << "IP camera was chosen\n";
cam_type = IP;
}
else if (strcmp("usb", argv[2]) == 0) {
std::cout << "USB camera was chosen\n";
cam_type = USB;
}
else {
std::cerr << "Unknown camera type was chosen: " << argv[2] << "\n";
return -2;
}
cv::VideoCapture video_src;
switch (cam_type) {
case IP:
// video_src.open(argv[3]);
video_src.open(0);
break;
case USB:
// video_src.open(atoi(argv[3]));
video_src.open("rtsp://*****:*****@192.168.1.64:554");
break;
default:
std::cerr << "Critical error\n";
return -3;
}
cv::Mat image;
cv::Mat prev;
cv::Mat current;
cv::Mat next;
cv::Rect motion_rect;
video_src.read(prev);
video_src.read(current);
video_src.read(next);
int color_space = CV_BGR2GRAY;
cv::Mat cs_prev;
cv::Mat cs_current;
cv::Mat cs_next;
cv::Mat diff1;
cv::Mat diff2;
cv::Mat result;
while (true) {
current.copyTo(prev);
next.copyTo(current);
video_src.read(next);
cv::cvtColor(prev, cs_prev, color_space);
cv::cvtColor(current, cs_current, color_space);
cv::cvtColor(next, cs_next, color_space);
cv::normalize(cs_prev, cs_prev, 0, 255, cv::NORM_MINMAX, CV_8UC1);
cv::normalize(cs_current, cs_current, 0, 255, cv::NORM_MINMAX, CV_8UC1);
cv::normalize(cs_next, cs_next, 0, 255, cv::NORM_MINMAX, CV_8UC1);
cv::resize(cs_prev, cs_prev, cv::Size(400, 400));
cv::resize(cs_current, cs_current, cv::Size(400, 400));
cv::resize(cs_next, cs_next, cv::Size(400, 400));
cv::absdiff(cs_next, cs_current, diff1);
cv::absdiff(cs_next, cs_prev, diff2);
cv::bitwise_and(diff1, diff2, result);
cv::imshow("bitwised", result);
cv::dilate(result, result, cv::Mat(), cv::Point(-1,-1), 3);
cv::GaussianBlur(result, result, cv::Size(21, 21), 0);
cv::imshow("GaussianBlured", result);
cv::threshold(result, result, 50, 255, CV_THRESH_BINARY);
// cv::imshow("thresholded", result);
// result = cv::Mat::zeros(cv::Size(400, 400), CV_8UC1);
// result.at<uchar>(100,100) = 255;
// result.at<uchar>(300,100) = 255;
// result.at<uchar>(100,300) = 255;
// result.at<uchar>(300,300) = 255;
// cv::erode(result, result, cv::Mat(), cv::Point(-1, -1), 3);
cv::dilate(result, result, cv::Mat(), cv::Point(-1,-1), 1);
// cv::imshow("dilate", result);
int x_start = 0;
int x_stop = cs_next.cols;
int y_start = 0;
int y_stop = cs_next.rows;
cv::Scalar color(255, 255, 255);
int max_deviation = 120;
cv::imshow("result", result);
cv::Rect ROI(x_start, y_start, x_stop - x_start, y_stop - y_start);
detectMotionRect(result, ROI, motion_rect, max_deviation);
cv::rectangle(cs_next, motion_rect, cv::Scalar(255), 3);
cv::imshow("imgae", cs_next);
char key = cv::waitKey(100) & 0xFF;
if (key == 'q') {
break;
}
}
return 0;
}
void DisparityProc::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC1); // Choose 8 BIT Format, e.g. TYPE_8UC1, MONO8, ...
}
catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
/* --------------------------------------------------------------------------------------------
Select ROI & Inititialize Masks
-------------------------------------------------------------------------------------------*/
cv::Mat image = cv_ptr->image;
cv::Mat ROI(image, cv::Rect(x_min, y_min, width+1, height+1));
cv::Mat X = cv::Mat::zeros(height+1, width+1, CV_32FC1);
cv::Mat Y = cv::Mat::zeros(height+1, width+1, CV_32FC1);
// Create Meshgrid
cv::Range x_bound(0, width);
cv::Range y_bound(0, height);
meshgrid(x_bound, y_bound, X, Y);
// Initialize and compute Masks
const float PI = 3.14159265358979f;
Line line1(-2.3, 175, 2); // Bicaval
Line line2(0.8, 20, 1); // Short Axis
Line line3(0, 60, 1); // 4 Chamber
Ellipse FO(50, 50, 35, 33, -50 * PI/180); // Constructor(x0, y0, a, b, phi)
Rectangle needle(0, 0 ,10, 10); // Constructor(x0, y0, width, height)
cv::Mat mask_line1;
cv::Mat mask_line2;
cv::Mat mask_line3;
cv::Mat mask_FO;
cv::Mat mask_needle;
//std::cerr << "X: " << X.rows << "x" << X.cols << std::endl;
//std::cerr << "Y: " << Y.rows << "x" << Y.cols << std::endl;
line1.calc_mask(mask_line1, X, Y);
line2.calc_mask(mask_line2, X, Y);
line3.calc_mask(mask_line3, X, Y);
FO.calc_mask(mask_FO, X, Y);
// Auto Configuration
// ROI = compute_config(ROI);
/* --------------------------------------------------------------------------------------------
Compute Start & End Points of Cuts
-------------------------------------------------------------------------------------------*/
std::pair<int,int> bicaval_start;
std::pair<int,int> bicaval_stop;
std::pair<int,int> short_axis_start;
std::pair<int,int> short_axis_stop;
std::pair<int,int> four_chamber_start;
std::pair<int,int> four_chamber_stop;
start_end_coord(line1, mask_FO, X, Y, bicaval_start, bicaval_stop);
start_end_coord(line2, mask_FO, X, Y, short_axis_start, short_axis_stop);
start_end_coord(line3, mask_FO, X, Y, four_chamber_start, four_chamber_stop);
/* --------------------------------------------------------------------------------------------
Compute Update of Tenting Curve
Main Idea: Minimize difference between SPOT & OLD SPOT and introduce some kind of spacial delay
-------------------------------------------------------------------------------------------*/
// Check whether or not tenting even occurs
bool th_exceeded = tenting_ocurrs(ROI, mask_FO);
// Update Spot Location if Threshold is exceeded
if (th_exceeded) {
// Estimate current position of needle
find_needle_tip(ROI, mask_FO, needle);
needle_tip_ = needle.get_mid();
if (old_spot_init) {
// Compute whether or not intersection occurs - for each Projection
std::pair<int,int> tmp_proj1 = line1.projection(needle_tip_, bicaval_start);
std::pair<int,int> tmp_proj2 = line2.projection(needle_tip_, short_axis_start);
std::pair<int,int> tmp_proj3 = line3.projection(needle_tip_, four_chamber_start);
bool moving1 = needle_moving(old_proj_bicaval, tmp_proj1);
bool moving2 = needle_moving(old_proj_short_axis, tmp_proj2);
bool moving3 = needle_moving(old_proj_four_chamber, tmp_proj3);
// Update Bicaval
if (!moving1) {
proj_bicaval = old_proj_bicaval; // Stay Still
}
else {
minimize_with_constraint(needle, line1, 0); // Minimize Scalar Product
}
// Update Short Axis
if (!moving2) {
proj_short_axis = old_proj_short_axis; // Stay Still
}
else {
minimize_with_constraint(needle, line2, 1); // Minimize Scalar Product
}
// Update 4 Chamber
if (!moving3) {
proj_four_chamber = old_proj_four_chamber; // Stay Still
}
else {
minimize_with_constraint(needle, line3, 2); // Minimize Scalar Product
}
}
else {
// Old Spot not initialized -> Initialize
initialize_spot(needle,line1, bicaval_start, 0);
initialize_spot(needle,line2, short_axis_start, 1);
initialize_spot(needle,line3, four_chamber_start, 2);
old_spot_init = true;
}
}
else {
//std::cerr << "Threshold NOT exceeded..." << std::endl;
// Draw straight line
proj_bicaval = bicaval_start;
proj_short_axis = short_axis_start;
proj_four_chamber = four_chamber_start;
old_spot_init = false;
}
// Derive x/y _ points (new 1D coord sys) from spot
derive_xy_points(bicaval_start, bicaval_stop, ROI, 0);
derive_xy_points(short_axis_start, short_axis_stop, ROI, 1);
derive_xy_points(four_chamber_start, four_chamber_stop, ROI, 2);
// Update Old Spot
old_proj_bicaval = line1.projection(proj_bicaval, bicaval_start);
old_proj_short_axis = line2.projection(proj_short_axis, short_axis_start);
old_proj_four_chamber = line3.projection(proj_four_chamber, four_chamber_start);
// Print Values of Needle Location
/*
std::cerr << "Needle Location:" << std::endl;
int x_print = needle_tip_.first;
int y_print = needle_tip_.second;
int value = ROI.at<uint8_t>(y_print, x_print);
std::cerr << "(X,Y) = (" << x_print << ", " << y_print << ") : " << value << std::endl;
*/
// For Debugging -> Draw on FO
ROI = draw_on_FO (
ROI,
mask_FO,
mask_line1,
mask_line2,
mask_line3,
bicaval_start,
bicaval_stop,
short_axis_start,
short_axis_stop,
four_chamber_start,
four_chamber_stop,
needle );
/* --------------------------------------------------------------------------------------------
Interpoplate the two parabolas & evaluate on a fine mesh
-------------------------------------------------------------------------------------------*/
/*
* depricated...
*
Parabola left(x_points[0], y_points[0], x_points[1], y_points[1]);
Parabola right(x_points[2], y_points[2], x_points[1], y_points[1]);
int N_vis = 1000;
std::vector<float> x_vis(N_vis, 0);
std::vector<float> y_vis(N_vis, 0);
float h = (x_points[2]-x_points[0])/(N_vis-1);
for(unsigned int i=0; i<x_vis.size(); ++i) {
x_vis[i] = x_points[0] + i*h;
if (x_vis[i] < x_points[1])
y_vis[i] = left.eval_para(x_vis[i]);
else
y_vis[i] = right.eval_para(x_vis[i]);
}
*/
/* --------------------------------------------------------------------------------------------
Create Ros - Messages and publish data
-------------------------------------------------------------------------------------------*/
// Write ROI to published image
cv_ptr->image = ROI;
// Build Messages - BICAVAL
std_msgs::Float64MultiArray bicaval_x;
std_msgs::Float64MultiArray bicaval_y;
bicaval_x.layout.dim.push_back(std_msgs::MultiArrayDimension());
bicaval_x.layout.dim[0].size = x_pt_bicaval.size();
bicaval_x.layout.dim[0].stride = 1;
bicaval_x.layout.dim[0].label = "Bicaval_X_Val";
bicaval_x.data.clear();
bicaval_x.data.insert(bicaval_x.data.end(), x_pt_bicaval.begin(), x_pt_bicaval.end());
bicaval_y.layout.dim.push_back(std_msgs::MultiArrayDimension());
bicaval_y.layout.dim[0].size = y_pt_bicaval.size();
bicaval_y.layout.dim[0].stride = 1;
bicaval_y.layout.dim[0].label = "Bicaval_Y_Val";
bicaval_y.data.clear();
bicaval_y.data.insert(bicaval_y.data.end(), y_pt_bicaval.begin(), y_pt_bicaval.end());
// Build Messages - SHORT AXIS
std_msgs::Float64MultiArray short_axis_x;
std_msgs::Float64MultiArray short_axis_y;
short_axis_x.layout.dim.push_back(std_msgs::MultiArrayDimension());
short_axis_x.layout.dim[0].size = x_pt_short_axis.size();
short_axis_x.layout.dim[0].stride = 1;
short_axis_x.layout.dim[0].label = "Short_Axis_X_Val";
short_axis_x.data.clear();
short_axis_x.data.insert(short_axis_x.data.end(), x_pt_short_axis.begin(), x_pt_short_axis.end());
short_axis_y.layout.dim.push_back(std_msgs::MultiArrayDimension());
short_axis_y.layout.dim[0].size = y_pt_short_axis.size();
short_axis_y.layout.dim[0].stride = 1;
short_axis_y.layout.dim[0].label = "Short_Axis_Y_Val";
short_axis_y.data.clear();
short_axis_y.data.insert(short_axis_y.data.end(), y_pt_short_axis.begin(), y_pt_short_axis.end());
// Build Messages - 4 CHAMBER
std_msgs::Float64MultiArray four_chamber_x;
std_msgs::Float64MultiArray four_chamber_y;
four_chamber_x.layout.dim.push_back(std_msgs::MultiArrayDimension());
four_chamber_x.layout.dim[0].size = x_pt_four_chamber.size();
four_chamber_x.layout.dim[0].stride = 1;
four_chamber_x.layout.dim[0].label = "Four_Chamber_X_Val";
four_chamber_x.data.clear();
four_chamber_x.data.insert(four_chamber_x.data.end(), x_pt_four_chamber.begin(), x_pt_four_chamber.end());
four_chamber_y.layout.dim.push_back(std_msgs::MultiArrayDimension());
four_chamber_y.layout.dim[0].size = y_pt_four_chamber.size();
four_chamber_y.layout.dim[0].stride = 1;
four_chamber_y.layout.dim[0].label = "Four_Chamber_Y_Val";
four_chamber_y.data.clear();
four_chamber_y.data.insert(four_chamber_y.data.end(), y_pt_four_chamber.begin(), y_pt_four_chamber.end());
// Build Messages - NEEDLE LOCATION
std_msgs::Float64MultiArray needle_loc_msg;
bool punctured = true;
std::pair<float,float> tmp_ = FO.map_to_unit_circle(needle_tip_);
tmp_.second = -tmp_.second;
needle_loc_msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
needle_loc_msg.layout.dim[0].size = 3;
needle_loc_msg.layout.dim[0].stride = 1;
needle_loc_msg.layout.dim[0].label = "Needle_Location";
needle_loc_msg.data.clear();
needle_loc_msg.data.push_back(punctured);
needle_loc_msg.data.push_back(tmp_.first);
needle_loc_msg.data.push_back(tmp_.second);
// Publish
image_pub_.publish(cv_ptr->toImageMsg());
bicaval_x_pub.publish(bicaval_x);
bicaval_y_pub.publish(bicaval_y);
short_axis_x_pub.publish(short_axis_x);
short_axis_y_pub.publish(short_axis_y);
four_chamber_x_pub.publish(four_chamber_x);
four_chamber_y_pub.publish(four_chamber_y);
needle_loc_pub.publish(needle_loc_msg);
}
