double ChannelHeadTracker::computeChannelShift(DblRasterMx & channelTrackPrev, DblRasterMx & channelTrackNew)
{
if (_lastID < 1 || _prevChannels.getColNr() < 1 || _prevChannels.getRowNr() < 1)
return 0;
DblRasterMx distances;
RasterPositionMatrix positions;
distanceTransform(channelTrackPrev, distances, positions);
DblRasterMx::iterator it1 = channelTrackNew.begin(), end = channelTrackNew.end();
DblRasterMx::iterator iDistences = distances.begin();
double shiftSum = 0.0;
for (; it1 != end; ++it1, ++iDistences) {
if (*it1 > 0.0)
shiftSum+=*iDistences;
}
return shiftSum;
}
std::vector<float> ShapeFit::fit(cv::Mat& image, float threshold) {
time_t start = clock();
// cretae a binary image
cv::Mat image2;
cv::threshold(image, image2, 254, 255, CV_THRESH_BINARY);
// compute a distance map
cv::Mat distMap;
cv::distanceTransform(image2, distMap, CV_DIST_L2, 3);
distMap.convertTo(distMap, CV_32F);
//cv::imwrite("distMap.png", distMap);
int WIDTH = image.cols;
int HEIGHT = image.rows;
// initialize the parameters
std::vector<float> params(4);
params[0] = WIDTH * 0.25;
params[1] = HEIGHT * 0.25;
params[2] = WIDTH * 0.5;
params[3] = HEIGHT * 0.5;
float delta = 10.0f;
int stepsize = 10;
int step_count = 0;
float dist = std::numeric_limits<float>::max();
for (int i = 0; i < 100; ++i) {
///////////////////////////////// DEBUG /////////////////////////////////
/*
cv::Mat hoge(HEIGHT, WIDTH, CV_8UC1, cv::Scalar(255));
cv::rectangle(hoge, cv::Rect(params[0], params[1], params[2], params[3]), cv::Scalar(0), 1, CV_AA);
char filename[256];
sprintf(filename, "results/image_%04d.png", i);
cv::imwrite(filename, hoge);
*/
///////////////////////////////// DEBUG /////////////////////////////////
int r = i % params.size();
float old_value = params[r];
// dist from the proposal 1
params[r] = std::max(0.0f, old_value - delta);
float dist1 = distanceTransform(distMap, params);
// dist from the proposal 2
params[r] = old_value + delta;
float dist2 = distanceTransform(distMap, params);
if (dist1 <= dist2 && dist1 <= dist) {
params[r] = std::max(0.0f, old_value - delta);
dist = dist1;
}
else if (dist2 <= dist1 && dist2 <= dist) {
params[r] = old_value + delta;
dist = dist2;
}
else {
params[r] = old_value;
}
if (step_count >= stepsize) {
step_count = 0;
delta = std::max(1.0f, delta * 0.8f);
}
if (dist < threshold) break;
}
time_t end = clock();
std::cout << "Duration: " << (double)(end - start) / CLOCKS_PER_SEC << "sec." << std::endl;
return params;
}
// A4
int HistologicalEntities::plSeparateNuclei(const Mat& img, const Mat& seg_open, Mat& seg_nonoverlap,
::cciutils::SimpleCSVLogger *logger, ::cciutils::cv::IntermediateResultHandler *iresHandler) {
/*
*
seg_big = imdilate(bwareaopen(seg_open,30),strel('disk',1));
*/
// bwareaopen is done as a area threshold.
int compcount2;
//#if defined (USE_UF_CCL)
Mat seg_big_t = ::nscale::bwareaopen2(seg_open, false, true, 30, std::numeric_limits<int>::max(), 8, compcount2);
// printf(" cpu compcount 30-1000 = %d\n", compcount2);
//#else
// Mat seg_big_t = ::nscale::bwareaopen<unsigned char>(seg_open, 30, std::numeric_limits<int>::max(), 8, compcount2);
//#endif
if (logger) logger->logTimeSinceLastLog("30To1000");
if (iresHandler) iresHandler->saveIntermediate(seg_big_t, 14);
Mat disk3 = getStructuringElement(MORPH_ELLIPSE, Size(3,3));
Mat seg_big = Mat::zeros(seg_big_t.size(), seg_big_t.type());
dilate(seg_big_t, seg_big, disk3);
if (iresHandler) iresHandler->saveIntermediate(seg_big, 15);
// imwrite("test/out-nucleicandidatesbig.ppm", seg_big);
if (logger) logger->logTimeSinceLastLog("dilate");
/*
*
distance = -bwdist(~seg_big);
distance(~seg_big) = -Inf;
distance2 = imhmin(distance, 1);
*
*
*/
// distance transform: matlab code is doing this:
// invert the image so nuclei candidates are holes
// compute the distance (distance of nuclei pixels to background)
// negate the distance. so now background is still 0, but nuclei pixels have negative distances
// set background to -inf
// really just want the distance map. CV computes distance to 0.
// background is 0 in output.
// then invert to create basins
Mat dist(seg_big.size(), CV_32FC1);
// imwrite("seg_big.pbm", seg_big);
// opencv: compute the distance to nearest zero
// matlab: compute the distance to the nearest non-zero
distanceTransform(seg_big, dist, CV_DIST_L2, CV_DIST_MASK_PRECISE);
double mmin, mmax;
minMaxLoc(dist, &mmin, &mmax);
if (iresHandler) iresHandler->saveIntermediate(dist, 16);
// invert and shift (make sure it's still positive)
//dist = (mmax + 1.0) - dist;
dist = - dist; // appears to work better this way.
// cciutils::cv::imwriteRaw("test/out-dist", dist);
// then set the background to -inf and do imhmin
//Mat distance = Mat::zeros(dist.size(), dist.type());
// appears to work better with -inf as background
Mat distance(dist.size(), dist.type(), -std::numeric_limits<float>::max());
dist.copyTo(distance, seg_big);
// cciutils::cv::imwriteRaw("test/out-distance", distance);
if (logger) logger->logTimeSinceLastLog("distTransform");
if (iresHandler) iresHandler->saveIntermediate(distance, 17);
// then do imhmin. (prevents small regions inside bigger regions)
// imwrite("in-imhmin.ppm", distance);
Mat distance2 = ::nscale::imhmin<float>(distance, 1.0f, 8);
if (logger) logger->logTimeSinceLastLog("imhmin");
if (iresHandler) iresHandler->saveIntermediate(distance2, 18);
// imwrite("distance2.ppm", dist);
//cciutils::cv::imwriteRaw("test/out-distanceimhmin", distance2);
/*
*
seg_big(watershed(distance2)==0) = 0;
seg_nonoverlap = seg_big;
*
*/
Mat nuclei = Mat::zeros(img.size(), img.type());
// Mat distance3 = distance2 + (mmax + 1.0);
// Mat dist4 = Mat::zeros(distance3.size(), distance3.type());
// distance3.copyTo(dist4, seg_big);
// Mat dist5(dist4.size(), CV_8U);
// dist4.convertTo(dist5, CV_8U, (std::numeric_limits<unsigned char>::max() / mmax));
// cvtColor(dist5, nuclei, CV_GRAY2BGR);
img.copyTo(nuclei, seg_big);
if (logger) logger->logTimeSinceLastLog("nucleiCopy");
if (iresHandler) iresHandler->saveIntermediate(nuclei, 19);
// watershed in openCV requires labels. input foreground > 0, 0 is background
// critical to use just the nuclei and not the whole image - else get a ring surrounding the regions.
//#if defined (USE_UF_CCL)
Mat watermask = ::nscale::watershed2(nuclei, distance2, 8);
//#else
// Mat watermask = ::nscale::watershed(nuclei, distance2, 8);
//#endif
// cciutils::cv::imwriteRaw("test/out-watershed", watermask);
if (logger) logger->logTimeSinceLastLog("watershed");
if (iresHandler) iresHandler->saveIntermediate(watermask, 20);
// MASK approach
seg_nonoverlap = Mat::zeros(seg_big.size(), seg_big.type());
seg_big.copyTo(seg_nonoverlap, (watermask >= 0));
if (logger) logger->logTimeSinceLastLog("water to mask");
if (iresHandler) iresHandler->saveIntermediate(seg_nonoverlap, 21);
//// ERODE has been replaced with border finding and moved into watershed.
// Mat twm(seg_nonoverlap.rows + 2, seg_nonoverlap.cols + 2, seg_nonoverlap.type());
// Mat t_nonoverlap = Mat::zeros(twm.size(), twm.type());
// //ERODE to fix border
// if (seg_nonoverlap.type() == CV_32S)
// copyMakeBorder(seg_nonoverlap, twm, 1, 1, 1, 1, BORDER_CONSTANT, Scalar_<int>(std::numeric_limits<int>::max()));
// else
// copyMakeBorder(seg_nonoverlap, twm, 1, 1, 1, 1, BORDER_CONSTANT, Scalar_<unsigned char>(std::numeric_limits<unsigned char>::max()));
// erode(twm, t_nonoverlap, disk3);
// seg_nonoverlap = t_nonoverlap(Rect(1,1,seg_nonoverlap.cols, seg_nonoverlap.rows));
// if (logger) logger->logTimeSinceLastLog("watershed erode");
// if (iresHandler) iresHandler->saveIntermediate(seg_nonoverlap, 22);
// twm.release();
// t_nonoverlap.release();
// LABEL approach -erode does not support 32S
// seg_nonoverlap = ::nscale::PixelOperations::replace<int>(watermask, (int)0, (int)-1);
// watershed output: 0 for background, -1 for border. bwlabel before watershd has 0 for background
return ::nscale::HistologicalEntities::CONTINUE;
}
void TRop::expandPaint(const TRasterCM32P &rasCM) {
distanceTransform(rasCM, SomePaint(), CopyPaint());
}
void VolumeDistanceTransform::process() {
distanceTransform();
}
int ChannelHeadTracker::track(const DblRasterMx & currentChannelHeads, DblRasterMx & currentChannels, double time)
{
DblRasterMx currentChannelHeadsCopy(currentChannelHeads);
DblRasterMx channelTrackPrev;
channelTrackPrev.initlike(currentChannels);
channelTrackPrev.fill(0.0);
DblRasterMx channelTrackNew;
channelTrackNew.initlike(currentChannels);
channelTrackNew.fill(0.0);
//first try to track the current channel heads:
IntRasterMx::iterator iChannelHeads = _channelHeads.begin(), endChannelHeads = _channelHeads.end();
int nr_of_captures = 0;
for (; iChannelHeads != endChannelHeads; ++iChannelHeads) {
int id = *iChannelHeads;
if (id > 0) {
size_t row = iChannelHeads.getRow();
size_t col = iChannelHeads.getCol();
DblRasterMx::iterator iCurrentChannelHead = currentChannelHeadsCopy.getIteratorAt(row, col);
// the channel head did not move
if (*iCurrentChannelHead > 1e-6) {
copyChannelTo(_prevChannels, row, col, channelTrackPrev, 1.0);
copyChannelTo(currentChannels, row, col, channelTrackNew, 1.0);
*iCurrentChannelHead = 0.0;
continue;
}
// check if the channel head moved
DblRasterMx::iterator::dCoords delta;
delta._nDCol = 0;
delta._nDRow = 0;
RasterPositionVector neighbourChannelHeadPositions;
neighbourChannelHeadPositions.reserve(8);
for (unsigned char cc = 1; cc < 10; ++cc) {
if (cc==5 || !iCurrentChannelHead.isValidItemByChainCode(cc))
continue;
double val = iCurrentChannelHead.chain_code(cc);
if (val > 1e-6) {
delta = iCurrentChannelHead.getChainCodeDelta(cc);
RasterPosition pos(row + delta._nDRow, col + delta._nDCol);
neighbourChannelHeadPositions.push_back(pos);
}
}
size_t n = neighbourChannelHeadPositions.size();
RasterPosition new_pos;
if (n == 0) { // the channel head is ereased
*iChannelHeads = 0;
_channelHeadMap[id].erease(time);
if (currentChannels(row, col) > 1e-6) {
++nr_of_captures;
}
continue;
} else if (n == 1) { // the channel head moved, and there is only one direction
new_pos = neighbourChannelHeadPositions.front();
} else if (n > 1) { // the channel head moved, but there are multiple directions
DblRasterMx channelPixels;
copyChannelTo(_prevChannels, row, col, channelPixels, 1.0);
DblRasterMx distances;
RasterPositionMatrix positions;
distanceTransform(channelPixels, distances, positions);
double minDistance = DoubleUtil::getMAXValue();
int minDistanceIndex = 0;
for (int i = 0; i < n; ++i ) {
RasterPosition & pos = neighbourChannelHeadPositions[i];
double distance = channelDistance( currentChannels, pos.getRow(), pos.getCol(), distances);
if (distance < minDistance ) {
minDistance = distance;
minDistanceIndex = i;
}
}
new_pos = neighbourChannelHeadPositions[minDistanceIndex];
}
copyChannelTo(_prevChannels, row, col, channelTrackPrev, 1.0);
copyChannelTo(currentChannels, new_pos.getRow(), new_pos.getCol(), channelTrackNew, 1.0);
*iChannelHeads = 0;
_channelHeads(new_pos.getRow(), new_pos.getCol()) = id;
_channelHeadMap[id].moveTo(new_pos, time);
currentChannelHeadsCopy(new_pos.getRow(), new_pos.getCol()) = 0.0;
}
}
_channelShift = computeChannelShift(channelTrackPrev, channelTrackNew);
// find the new channel heads
DblRasterMx::iterator iCurrentChannelHead = currentChannelHeadsCopy.begin(), endCurrentChannelHead = currentChannelHeadsCopy.end();
for (; iCurrentChannelHead != endCurrentChannelHead; ++iCurrentChannelHead) {
if (*iCurrentChannelHead > 1e-6) {
RasterPosition pos(iCurrentChannelHead.getRow(), iCurrentChannelHead.getCol());
++_lastID;
_channelHeadMap[_lastID] = ChannelHead(_lastID, pos, time);
_channelHeads(pos.getRow(), pos.getCol()) = _lastID;
}
}
_prevChannels = currentChannels;
return nr_of_captures;
}
std::pair<std::vector<StateOfCar>, Seed> AStarSeed::findPathToTargetWithAstar(const cv::Mat& img,const State& start,const State& goal) {
// USE : for garanteed termination of planner
int no_of_iterations = 0;
fusionMap = img;
MAP_MAX_ROWS = img.rows;
MAP_MAX_COLS = img.cols;
if(DT==1)
distanceTransform();
if (DEBUG) {
image = fusionMap.clone();
}
StateOfCar startState(start), targetState(goal);
std::map<StateOfCar, open_map_element> openMap;
std::map<StateOfCar,StateOfCar, comparatorMapState> came_from;
SS::PriorityQueue<StateOfCar> openSet;
openSet.push(startState);
if (startState.isCloseTo(targetState)) {
std::cout<<"Bot is On Target"<<std::endl;
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
else if(isOnTheObstacle(startState)){
std::cout<<"Bot is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
else if(isOnTheObstacle(targetState)){
std::cout<<"Target is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
while (!openSet.empty() && ros::ok()) {
// std::cout<<"openSet size : "<<openSet.size()<<"\n";
if(no_of_iterations > MAX_ITERATIONS){
std::cerr<<"Overflow : openlist size : "<<openSet.size()<<"\n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
StateOfCar currentState=openSet.top();
if (openMap.find(currentState)!=openMap.end() && openMap[currentState].membership == CLOSED) {
openSet.pop();
}
currentState=openSet.top();
if (DEBUG) {
std::cout<<"current x : "<<currentState.x()<<" current y : "<<currentState.y()<<std::endl;
plotPointInMap(currentState);
cv::imshow("[PLANNER] Map", image);
cvWaitKey(0);
}
// TODO : use closeTo instead of onTarget
if (currentState.isCloseTo(targetState)) {
std::cout<<"openSet size : "<<openSet.size()<<"\n";
// std::cout<<"Target Reached"<<std::endl;
return reconstructPath(currentState, came_from);
}
openSet.pop();
openMap[currentState].membership = UNASSIGNED;
openMap[currentState].cost=-currentState.gCost();
openMap[currentState].membership=CLOSED;
std::vector<StateOfCar> neighborsOfCurrentState = neighborNodesWithSeeds(currentState);
for (unsigned int iterator=0; iterator < neighborsOfCurrentState.size(); iterator++) {
StateOfCar neighbor = neighborsOfCurrentState[iterator];
double tentativeGCostAlongFollowedPath = neighbor.gCost() + currentState.gCost();
double admissible = neighbor.distanceTo(targetState);
double consistent = admissible;
double intensity = fusionMap.at<uchar>(neighbor.y(), neighbor.x());
double consistentAndIntensity = (neighbor.hCost()*neighbor.hCost()+2 + intensity*intensity)/(neighbor.hCost()+intensity+2);
if (!((openMap.find(neighbor) != openMap.end()) &&
(openMap[neighbor].membership == OPEN))) {
came_from[neighbor] = currentState;
neighbor.gCost( tentativeGCostAlongFollowedPath) ;
if(DT==1)
neighbor.hCost(consistentAndIntensity);
else
neighbor.hCost( consistent) ;
neighbor.updateTotalCost();
openSet.push(neighbor);
openMap[neighbor].membership = OPEN;
openMap[neighbor].cost = neighbor.gCost();
}
}
no_of_iterations++;
}
std::cerr<<"NO PATH FOUND"<<std::endl;
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
void RecognitionDemos( Mat& full_image, Mat& template1, Mat& template2, Mat& template1locations, Mat& template2locations, VideoCapture& bicycle_video, Mat& bicycle_background, Mat& bicycle_model, VideoCapture& people_video, CascadeClassifier& cascade, Mat& numbers, Mat& good_orings, Mat& bad_orings, Mat& unknown_orings )
{
Timestamper* timer = new Timestamper();
// Principal Components Analysis
PCASimpleExample();
char ch = cvWaitKey();
cvDestroyAllWindows();
PCAFaceRecognition();
ch = cvWaitKey();
cvDestroyAllWindows();
// Statistical Pattern Recognition
Mat gray_numbers,binary_numbers;
cvtColor(numbers, gray_numbers, CV_BGR2GRAY);
threshold(gray_numbers,binary_numbers,128,255,THRESH_BINARY_INV);
vector<vector<Point>> contours;
vector<Vec4i> hierarchy;
findContours(binary_numbers,contours,hierarchy,CV_RETR_TREE,CV_CHAIN_APPROX_NONE);
Mat contours_image = Mat::zeros(binary_numbers.size(), CV_8UC3);
contours_image = Scalar(255,255,255);
// Do some processing on all contours (objects and holes!)
vector<RotatedRect> min_bounding_rectangle(contours.size());
vector<vector<Point>> hulls(contours.size());
vector<vector<int>> hull_indices(contours.size());
vector<vector<Vec4i>> convexity_defects(contours.size());
vector<Moments> contour_moments(contours.size());
for (int contour_number=0; (contour_number<(int)contours.size()); contour_number++)
{
if (contours[contour_number].size() > 10)
{
min_bounding_rectangle[contour_number] = minAreaRect(contours[contour_number]);
convexHull(contours[contour_number], hulls[contour_number]);
convexHull(contours[contour_number], hull_indices[contour_number]);
convexityDefects( contours[contour_number], hull_indices[contour_number], convexity_defects[contour_number]);
contour_moments[contour_number] = moments( contours[contour_number] );
}
}
for (int contour_number=0; (contour_number>=0); contour_number=hierarchy[contour_number][0])
{
if (contours[contour_number].size() > 10)
{
Scalar colour( rand()&0x7F, rand()&0x7F, rand()&0x7F );
drawContours( contours_image, contours, contour_number, colour, CV_FILLED, 8, hierarchy );
char output[500];
double area = contourArea(contours[contour_number])+contours[contour_number].size()/2+1;
// Process any holes (removing the area from the are of the enclosing contour)
for (int hole_number=hierarchy[contour_number][2]; (hole_number>=0); hole_number=hierarchy[hole_number][0])
{
area -= (contourArea(contours[hole_number])-contours[hole_number].size()/2+1);
Scalar colour( rand()&0x7F, rand()&0x7F, rand()&0x7F );
drawContours( contours_image, contours, hole_number, colour, CV_FILLED, 8, hierarchy );
sprintf(output,"Area=%.0f", contourArea(contours[hole_number])-contours[hole_number].size()/2+1);
Point location( contours[hole_number][0].x +20, contours[hole_number][0].y +5 );
putText( contours_image, output, location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
}
// Draw the minimum bounding rectangle
Point2f bounding_rect_points[4];
min_bounding_rectangle[contour_number].points(bounding_rect_points);
line( contours_image, bounding_rect_points[0], bounding_rect_points[1], Scalar(0, 0, 127));
line( contours_image, bounding_rect_points[1], bounding_rect_points[2], Scalar(0, 0, 127));
line( contours_image, bounding_rect_points[2], bounding_rect_points[3], Scalar(0, 0, 127));
line( contours_image, bounding_rect_points[3], bounding_rect_points[0], Scalar(0, 0, 127));
float bounding_rectangle_area = min_bounding_rectangle[contour_number].size.area();
// Draw the convex hull
drawContours( contours_image, hulls, contour_number, Scalar(127,0,127) );
// Highlight any convexities
int largest_convexity_depth=0;
for (int convexity_index=0; convexity_index < (int)convexity_defects[contour_number].size(); convexity_index++)
{
if (convexity_defects[contour_number][convexity_index][3] > largest_convexity_depth)
largest_convexity_depth = convexity_defects[contour_number][convexity_index][3];
if (convexity_defects[contour_number][convexity_index][3] > 256*2)
{
line( contours_image, contours[contour_number][convexity_defects[contour_number][convexity_index][0]], contours[contour_number][convexity_defects[contour_number][convexity_index][2]], Scalar(0,0, 255));
line( contours_image, contours[contour_number][convexity_defects[contour_number][convexity_index][1]], contours[contour_number][convexity_defects[contour_number][convexity_index][2]], Scalar(0,0, 255));
}
}
double hu_moments[7];
HuMoments( contour_moments[contour_number], hu_moments );
sprintf(output,"Perimeter=%d, Area=%.0f, BArea=%.0f, CArea=%.0f", contours[contour_number].size(),area,min_bounding_rectangle[contour_number].size.area(),contourArea(hulls[contour_number]));
Point location( contours[contour_number][0].x, contours[contour_number][0].y-3 );
putText( contours_image, output, location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
sprintf(output,"HuMoments = %.2f, %.2f, %.2f", hu_moments[0],hu_moments[1],hu_moments[2]);
Point location2( contours[contour_number][0].x+100, contours[contour_number][0].y-3+15 );
putText( contours_image, output, location2, FONT_HERSHEY_SIMPLEX, 0.4, colour );
}
}
imshow("Shape Statistics", contours_image );
char c = cvWaitKey();
cvDestroyAllWindows();
// Support Vector Machine
imshow("Good - original",good_orings);
imshow("Defective - original",bad_orings);
imshow("Unknown - original",unknown_orings);
SupportVectorMachineDemo(good_orings,"Good",bad_orings,"Defective",unknown_orings);
c = cvWaitKey();
cvDestroyAllWindows();
// Template Matching
Mat display_image, correlation_image;
full_image.copyTo( display_image );
double min_correlation, max_correlation;
Mat matched_template_map;
int result_columns = full_image.cols - template1.cols + 1;
int result_rows = full_image.rows - template1.rows + 1;
correlation_image.create( result_columns, result_rows, CV_32FC1 );
timer->reset();
double before_tick_count = static_cast<double>(getTickCount());
matchTemplate( full_image, template1, correlation_image, CV_TM_CCORR_NORMED );
double after_tick_count = static_cast<double>(getTickCount());
double duration_in_ms = 1000.0*(after_tick_count-before_tick_count)/getTickFrequency();
minMaxLoc( correlation_image, &min_correlation, &max_correlation );
FindLocalMaxima( correlation_image, matched_template_map, max_correlation*0.99 );
timer->recordTime("Template Matching (1)");
Mat matched_template_display1;
cvtColor(matched_template_map, matched_template_display1, CV_GRAY2BGR);
Mat correlation_window1 = convert_32bit_image_for_display( correlation_image, 0.0 );
DrawMatchingTemplateRectangles( display_image, matched_template_map, template1, Scalar(0,0,255) );
double precision, recall, accuracy, specificity, f1;
Mat template1locations_gray;
cvtColor(template1locations, template1locations_gray, CV_BGR2GRAY);
CompareRecognitionResults( matched_template_map, template1locations_gray, precision, recall, accuracy, specificity, f1 );
char results[400];
Scalar colour( 255, 255, 255);
sprintf( results, "precision=%.2f", precision);
Point location( 7, 213 );
putText( display_image, "Results (1)", location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
location.y += 13;
putText( display_image, results, location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
sprintf( results, "recall=%.2f", recall);
location.y += 13;
putText( display_image, results, location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
sprintf( results, "accuracy=%.2f", accuracy);
location.y += 13;
putText( display_image, results, location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
sprintf( results, "specificity=%.2f", specificity);
location.y += 13;
putText( display_image, results, location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
sprintf( results, "f1=%.2f", f1);
location.y += 13;
putText( display_image, results, location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
result_columns = full_image.cols - template2.cols + 1;
result_rows = full_image.rows - template2.rows + 1;
correlation_image.create( result_columns, result_rows, CV_32FC1 );
timer->ignoreTimeSinceLastRecorded();
matchTemplate( full_image, template2, correlation_image, CV_TM_CCORR_NORMED );
minMaxLoc( correlation_image, &min_correlation, &max_correlation );
FindLocalMaxima( correlation_image, matched_template_map, max_correlation*0.99 );
timer->recordTime("Template Matching (2)");
Mat matched_template_display2;
cvtColor(matched_template_map, matched_template_display2, CV_GRAY2BGR);
Mat correlation_window2 = convert_32bit_image_for_display( correlation_image, 0.0 );
DrawMatchingTemplateRectangles( display_image, matched_template_map, template2, Scalar(0,0,255) );
timer->putTimes(display_image);
Mat template2locations_gray;
cvtColor(template2locations, template2locations_gray, CV_BGR2GRAY);
CompareRecognitionResults( matched_template_map, template2locations_gray, precision, recall, accuracy, specificity, f1 );
sprintf( results, "precision=%.2f", precision);
location.x = 123;
location.y = 213;
putText( display_image, "Results (2)", location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
location.y += 13;
putText( display_image, results, location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
sprintf( results, "recall=%.2f", recall);
location.y += 13;
putText( display_image, results, location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
sprintf( results, "accuracy=%.2f", accuracy);
location.y += 13;
putText( display_image, results, location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
sprintf( results, "specificity=%.2f", specificity);
location.y += 13;
putText( display_image, results, location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
sprintf( results, "f1=%.2f", f1);
location.y += 13;
putText( display_image, results, location, FONT_HERSHEY_SIMPLEX, 0.4, colour );
Mat correlation_display1, correlation_display2;
cvtColor(correlation_window1, correlation_display1, CV_GRAY2BGR);
cvtColor(correlation_window2, correlation_display2, CV_GRAY2BGR);
Mat output1 = JoinImagesVertically(template1,"Template (1)",correlation_display1,"Correlation (1)",4);
Mat output2 = JoinImagesVertically(output1,"",matched_template_display1,"Local maxima (1)",4);
Mat output3 = JoinImagesVertically(template2,"Template (2)",correlation_display2,"Correlation (2)",4);
Mat output4 = JoinImagesVertically(output3,"",matched_template_display2,"Local maxima (2)",4);
Mat output5 = JoinImagesHorizontally( full_image, "Original Image", output2, "", 4 );
Mat output6 = JoinImagesHorizontally( output5, "", output4, "", 4 );
Mat output7 = JoinImagesHorizontally( output6, "", display_image, "", 4 );
imshow( "Template matching result", output7 );
c = cvWaitKey();
cvDestroyAllWindows();
// Chamfer Matching
Mat model_gray,model_edges,model_edges2;
cvtColor(bicycle_model, model_gray, CV_BGR2GRAY);
threshold(model_gray,model_edges,127,255,THRESH_BINARY);
Mat current_frame;
bicycle_video.set(CV_CAP_PROP_POS_FRAMES,400); // Just in case the video has already been used.
bicycle_video >> current_frame;
bicycle_background = current_frame.clone();
bicycle_video.set(CV_CAP_PROP_POS_FRAMES,500);
timer->reset();
int count = 0;
while (!current_frame.empty() && (count < 8))
{
Mat result_image = current_frame.clone();
count++;
Mat difference_frame, difference_gray, current_edges;
absdiff(current_frame,bicycle_background,difference_frame);
cvtColor(difference_frame, difference_gray, CV_BGR2GRAY);
Canny(difference_frame, current_edges, 100, 200, 3);
vector<vector<Point> > results;
vector<float> costs;
threshold(model_gray,model_edges,127,255,THRESH_BINARY);
Mat matching_image, chamfer_image, local_minima;
timer->ignoreTimeSinceLastRecorded();
threshold(current_edges,current_edges,127,255,THRESH_BINARY_INV);
distanceTransform( current_edges, chamfer_image, CV_DIST_L2 , 3);
timer->recordTime("Chamfer Image");
ChamferMatching( chamfer_image, model_edges, matching_image );
timer->recordTime("Matching");
FindLocalMinima( matching_image, local_minima, 500.0 );
timer->recordTime("Find Minima");
DrawMatchingTemplateRectangles( result_image, local_minima, model_edges, Scalar( 255, 0, 0 ) );
Mat chamfer_display_image = convert_32bit_image_for_display( chamfer_image );
Mat matching_display_image = convert_32bit_image_for_display( matching_image );
//timer->putTimes(result_image);
Mat current_edges_display, local_minima_display, model_edges_display, colour_matching_display_image, colour_chamfer_display_image;
cvtColor(current_edges, current_edges_display, CV_GRAY2BGR);
cvtColor(local_minima, local_minima_display, CV_GRAY2BGR);
cvtColor(model_edges, model_edges_display, CV_GRAY2BGR);
cvtColor(matching_display_image, colour_matching_display_image, CV_GRAY2BGR);
cvtColor(chamfer_display_image, colour_chamfer_display_image, CV_GRAY2BGR);
Mat output1 = JoinImagesVertically(current_frame,"Video Input",current_edges_display,"Edges from difference", 4);
Mat output2 = JoinImagesVertically(output1,"",model_edges_display,"Model", 4);
Mat output3 = JoinImagesVertically(bicycle_background,"Static Background",colour_chamfer_display_image,"Chamfer image", 4);
Mat output4 = JoinImagesVertically(output3,"",colour_matching_display_image,"Degree of fit", 4);
Mat output5 = JoinImagesVertically(difference_frame,"Difference",result_image,"Result", 4);
Mat output6 = JoinImagesVertically(output5,"",local_minima_display,"Local minima", 4);
Mat output7 = JoinImagesHorizontally( output2, "", output4, "", 4 );
Mat output8 = JoinImagesHorizontally( output7, "", output6, "", 4 );
imshow("Chamfer matching", output8);
c = waitKey(1000); // This makes the image appear on screen
bicycle_video >> current_frame;
}
c = cvWaitKey();
cvDestroyAllWindows();
// Cascade of Haar classifiers (most often shown for face detection).
VideoCapture camera;
camera.open(1);
camera.set(CV_CAP_PROP_FRAME_WIDTH, 320);
camera.set(CV_CAP_PROP_FRAME_HEIGHT, 240);
if( camera.isOpened() )
{
timer->reset();
Mat current_frame;
do {
camera >> current_frame;
if( current_frame.empty() )
break;
vector<Rect> faces;
timer->ignoreTimeSinceLastRecorded();
Mat gray;
cvtColor( current_frame, gray, CV_BGR2GRAY );
equalizeHist( gray, gray );
cascade.detectMultiScale( gray, faces, 1.1, 2, CV_HAAR_SCALE_IMAGE, Size(30, 30) );
timer->recordTime("Haar Classifier");
for( int count = 0; count < (int)faces.size(); count++ )
rectangle(current_frame, faces[count], cv::Scalar(255,0,0), 2);
//timer->putTimes(current_frame);
imshow( "Cascade of Haar Classifiers", current_frame );
c = waitKey(10); // This makes the image appear on screen
} while (c == -1);
}
void createWeightMap(const Mat &mask, float sharpness, Mat &weight)
{
CV_Assert(mask.type() == CV_8U);
distanceTransform(mask, weight, DIST_L1, 3);
threshold(weight * sharpness, weight, 1.f, 1.f, THRESH_TRUNC);
}
int main(int argc, char **argv){
if(argc != 9){
printf("wrong # args\n");
printf("inputfile outDir(./lol/) invert (0 or 1) k*1000 windowRadius gaussPyramidThresh(>= survive) finalMixThresh(>= survive) writeDebugImages(0 or 1)\n");
}
int width, height, channels;
unsigned char *data;
char *dir = argv[2];
int inv = atoi(argv[3]);
float k = float(atoi(argv[4]))/1000.f;
int window = atoi(argv[5]);
int gaussPyramidThresh = atoi(argv[6]);
int finalMixThresh = atoi(argv[7]);
bool debugImages = atoi(argv[8]);
bool res = loadImage(argv[1], &width, &height, &channels, &data);
if(!res){
printf("error reading image\n");
return 1;
}
printf("start\n");
// to grayscale
unsigned char *dataGray = new unsigned char[width*height];
toGrayscale(width,height,data,dataGray);
// build SAT
unsigned int *sat = new unsigned int[width*height];
float * satSquared = new float[width*height];
SAT(dataGray,sat,width,height);
SATSquared(dataGray,satSquared,width,height);
// do thresh
thresh(dataGray, sat, satSquared, width, height, k, window);
if(inv){invert(dataGray,width,height);}
char filename[200];
sprintf(filename,"%sresRaw.bmp",dir);
if(debugImages){saveImage(filename, width, height, 1, dataGray);}
unsigned char ** pyramid=NULL;
makePyramid(dataGray, &pyramid, pyramidLevels, gaussPyramidThresh, width,height);
for(int L=0; L<pyramidLevels; L++){
char filename[200];
sprintf(filename,"%sres_raw_%d.bmp",dir,L);
if(debugImages){saveImage(filename,width/pow(2,L),height/pow(2,L),1,pyramid[L]);}
}
// label
int vecSizeY=32;
int vecSizeX=128;
unsigned char *dist=new unsigned char[width*height];
unsigned short *labels = new unsigned short[width*height];
unsigned short area[maxLabels];
unsigned char *reason = new unsigned char[width*height];
unsigned char *tile = new unsigned char[width*height];
for(int l = pyramidLevels-1; l>=0; l--){
int lw = width/pow(2,l);
int lh = height/pow(2,l);
// write out debug data
char filename[200];
sprintf(filename,"%sres_%dp2.bmp",dir,l);
if(debugImages){saveImage(filename, lw,lh, 1, pyramid[l]);}
}
for(int L = pyramidLevels-1; L>=0; L--){
int lw = width/pow(2,L);
int lh = height/pow(2,L);
// clear out labels so that we can do progressive cleanup passes
for(int i=0; i<lw*lh; i++){reason[i]=0;labels[i]=0;}
unsigned short firstId = 1;
int tileId = 0;
int minArea = 6;
if(L<2){minArea = 30;}
int nTilesX = ceil((float)lw/(float)vecSizeX);
int nTilesY = ceil((float)lh/(float)vecSizeY);
int lastFixup = 0;
for(int by=0; by<nTilesY; by++){
int endY = (by+1)*vecSizeY;
if(endY>lh){endY=lh;}
bool fixupRanThisRow = false;
for(int bx=0; bx<nTilesX; bx++){
int endX = (bx+1)*vecSizeX;
if(endX>lw){endX=lw;}
label(pyramid[L],labels,reason,area,lw,lh,minArea,vecSizeX*bx,endX,vecSizeY*by,endY,maxLabels);
unsigned short lastId=0;
if(!condenseLabels(labels,area,firstId,&lastId,lw,lh,vecSizeX*bx,endX,vecSizeY*by,endY,maxLabels)){
printf("Error: ran out of labels!\n");
goto writeout; // an exception occured
}
firstId=lastId+1;
//printf("ML %d\n",(int)firstId);
// for debugging
for(int x=vecSizeX*bx; x<endX; x++){
for(int y=vecSizeY*by; y<endY; y++){
tile[x+y*lw]=tileId;
}
}
tileId++;
if(firstId > (maxLabels*4)/5 && !fixupRanThisRow){
labelProp(labels,area,lw,lh,0,lw,0,endY,maxLabels);
filter(pyramid,L,reason,labels,minArea,lw,lh,width,0,lw,lastFixup,endY,maxLabels);
computeArea(labels,area,lw,lh,0,lw,0,endY,maxLabels);
condenseLabels(labels,area,1,&firstId,lw,lh,0,lw,0,endY,maxLabels);
firstId++;
printf("fixup TL %d\n",firstId);
lastFixup = (by+1)*vecSizeY;
fixupRanThisRow=true;
}
}
}
// fix labels across region boundries
labelProp(labels,area,lw,lh,0,lw,0,lh,maxLabels);
computeArea(labels,area,lw,lh,0,lw,0,lh,maxLabels);
condenseLabels(labels,area,1,&firstId,lw,lh,0,lw,0,lh,maxLabels);
//printf("TL %d\n",firstId);
distanceTransform(pyramid[L],dist,0,lw,lh);
filter(pyramid,L,reason,labels,minArea,lw,lh,width,0,lw,0,lh,maxLabels);
// now what's left "must" be text, so delete it from other pyrmid levels and save it
writeout:
// write out debug data
char filename[200];
if(debugImages){
sprintf(filename,"%sL_%d.bmp",dir,L);
saveImage(filename, lw,lh, labels);
sprintf(filename,"%sD_%d.bmp",dir,L);
saveImage(filename, lw,lh, 1, dist);
sprintf(filename,"%sres_%d.bmp",dir,L);
saveImage(filename, lw,lh, 1, pyramid[L]);
sprintf(filename,"%sR_%d.bmp",dir,L);
saveImage(filename, lw,lh, 1, reason);
sprintf(filename,"%sT_%d.bmp",dir,L);
saveImage(filename, lw,lh, 1, tile);
}
if(L==pyramidLevels-1){
for(int l = 2; l>=0; l--){
int lw = width/pow(2,l);
int lh = height/pow(2,l);
// write out debug data
char filename[200];
sprintf(filename,"%sres_%dp.bmp",dir,l);
if(debugImages){saveImage(filename, lw,lh, 1, pyramid[l]);}
}
}
}
for(int y=0; y<height; y++){
for(int x=0; x<width; x++){
int count=0;
for(int l=0; l<pyramidLevels; l++){
int lx = x/pow(2,l);
int ly = y/pow(2,l);
int lw = width/pow(2,l);
if(pyramid[l][lx+ly*lw]){count++;}
}
if(count<finalMixThresh){
dataGray[x+y*width]=0;
}
}
}
sprintf(filename,"%sres.bmp",dir);
saveImage(filename, width, height, 1, dataGray);
return 0;
}
std::pair <std::vector<StateOfCar>, Seed> AStarSeed::findPathToTarget(const cv::Mat& map, const State& start, const State& goal, const int distance_transform, const int debug_current_state, int& status) {
// USE : for guaranteed termination of planner
int no_of_iterations = 0;
int max_iterations = 10000;
node_handle.getParam("local_planner/max_iterations", max_iterations);
fusion_map = map;
map_max_rows = map.rows;
map_max_cols = map.cols;
if (distance_transform == 1) {
distanceTransform();
}
if (debug_current_state) {
image = fusion_map.clone();
}
StateOfCar start_state(start), target_state(goal);
std::map<StateOfCar, open_map_element> open_map;
std::map<StateOfCar, StateOfCar, comparatorMapState> came_from;
SS::PriorityQueue<StateOfCar> open_set;
open_set.push(start_state);
if (start_state.isCloseTo(target_state)) {
status = 1;
return std::make_pair(std::vector<StateOfCar>(), Seed());
} else if (isOnTheObstacle(start_state)) {
std::cout << "Bot is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
} else if (isOnTheObstacle(target_state)) {
std::cout << "Target is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
while (!open_set.empty() && ros::ok()) {
if (no_of_iterations > max_iterations) {
status = open_set.size();
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
StateOfCar current_state = open_set.top();
if (open_map.find(current_state) != open_map.end() && open_map[current_state].membership == CLOSED) {
open_set.pop();
}
current_state = open_set.top();
if (debug_current_state) {
std::cout << "current x : " << current_state.x() << " current y : " << current_state.y() << std::endl;
plotPointInMap(current_state);
cv::imshow("[PLANNER] Map", image);
cvWaitKey(0);
}
// TODO : use closeTo instead of onTarget
if (current_state.isCloseTo(target_state)) {
status = 2;
return reconstructPath(current_state, came_from);
}
open_set.pop();
open_map[current_state].membership = UNASSIGNED;
open_map[current_state].cost = -current_state.gCost();
open_map[current_state].membership = CLOSED;
std::vector<StateOfCar> neighbors_of_current_state = neighborNodesWithSeeds(current_state);
for (unsigned int iterator = 0; iterator < neighbors_of_current_state.size(); iterator++) {
StateOfCar neighbor = neighbors_of_current_state[iterator];
double tentative_gcost_along_followed_path = neighbor.gCost() + current_state.gCost();
double admissible = neighbor.distanceTo(target_state);
double consistent = admissible;
if (!((open_map.find(neighbor) != open_map.end()) &&
(open_map[neighbor].membership == OPEN))) {
came_from[neighbor] = current_state;
neighbor.hCost(consistent);
neighbor.gCost(tentative_gcost_along_followed_path);
neighbor.updateTotalCost();
open_set.push(neighbor);
open_map[neighbor].membership = OPEN;
open_map[neighbor].cost = neighbor.gCost();
}
}
no_of_iterations++;
}
status = 0;
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
