void Body::applyForce(Vec2 f, Point p) {
if (length(f) == 0)
return;
if (p != getMassCenter() && !fixedAngle) {
Vec2 n = getMassCenter()-p;
force += getOrientedVec(n, length(f)*getCos(f, n));
momentium += getSin(f,n)*length(f)*distance(p, getMassCenter());
} else {
force += f;
}
}
void TargetExtractor::fill(int ksize, int threshold)
{
int r = (ksize - 1) / 2;
if (r <= 0) {
return;
}
Mat density;
calcDensity(mMask, density, ksize);
double half = ksize / 2.0, dist = ksize / 5.0;
int max = ksize * ksize * 9 / 10;
for (int i = r; i < mMask.rows - r; i++) {
for (int j = r; j < mMask.cols - r; j++) {
int count = density.at<int>(i, j);
if (count > max) {
mMask.at<uchar>(i, j) = 255;
} else if (count >= threshold) {
// TODO: further optimize the mass-center calculation
Point center;
Rect rect(j - r, i - r, ksize, ksize);
getMassCenter(mMask(rect), center);
if (abs(center.x - half) < dist && abs(center.y - half) < dist) {
mMask.at<uchar>(i, j) = 255;
}
}
}
}
}
Skeleton::Skeleton(cv::Mat skeletonizedImage, cv::Mat normalImage){
//QList<LabeledPoint> startList;
QList<cv::Point2i> dummyJunctionList;
QList<LabeledPoint> removedPoints;
QList<int> survivors;
int currentLabel = 1;
// SEARCH FOR ALL CURRENT BRANCHES
for (int x = 0; x < skeletonizedImage.cols; x++){
for (int y = 0; y < skeletonizedImage.rows; y++){
if (skeletonizedImage.at<uchar>(y, x) == 255){
int count = 0;
QVector<cv::Point2i> list;
for (int i = -1; i <= 1; i++){
for (int j = -1; j <= 1; j++){
if (i != 0 || j != 0){
if (y+j >= 0 && y+j < skeletonizedImage.rows && x+i >= 0 && x+i < skeletonizedImage.cols && skeletonizedImage.at<uchar>(y+j, x+i) > 0){
bool neighbourg = false;
cv::Point2i point(x+i,y+j);
for (int k = 0; k < list.size(); k++){
if (cv::norm(point - list[k]) == 1){
neighbourg = true;
}
}
if (!neighbourg){
count++;
list.append(point);
}
}
}
}
}
if (count == 1){
cv::Point2i point(x,y);
LabeledPoint lp;
lp.label = currentLabel;
lp.point = point;
startList.append(lp);
currentLabel++;
}
else if (count > 2){
cv::Point2i point(x,y);
dummyJunctionList.append(point);
}
}
}
}
// GO THROUGH THE BRANCH AND REMOVE ONE PIXEL AT A TIME
for (int i = 0; i < startList.size(); i++){
cv::Point2i current = startList[i].point;
int label = startList[i].label;
int round = 0;
bool done = false;
do {
LabeledPoint lp;
lp.point = current;
lp.label = label;
removedPoints.append(lp);
skeletonizedImage.at<uchar>(current.y, current.x) = 0;
int count = 0;
int x = current.x;
int y = current.y;
QVector<cv::Point2i> list;
for (int i = -1; i <= 1; i++){
for (int j = -1; j <= 1; j++){
if (i != 0 || j != 0){
if (y+j >= 0 && y+j < skeletonizedImage.rows && x+i >= 0 && x+i < skeletonizedImage.cols && skeletonizedImage.at<uchar>(y+j, x+i) == 255){
bool junction = false;
cv::Point2i point(x+i,y+j);
for (int k = 0; k < dummyJunctionList.size(); k++){
if (cv::norm(point - dummyJunctionList[k]) <= 1){
junction = true;
}
}
if (!junction){
count++;
list.append(point);
}
}
}
}
}
if (count >= 1){
current = list[0];
}
round ++;
done = (count < 1);
if (round == 12){
survivors.append(label);
}
} while(round < 12 && !done);
}
// IF THE BRANCH IS STILL LONG ENOUGH, REDRAW IT
for (int i = 0; i < removedPoints.size(); i++){
cv::Point2i point = removedPoints[i].point;
int label = removedPoints[i].label;
if (survivors.contains(label)){
skeletonizedImage.at<uchar>(point) = 255;
}
}
// LOOPS
std::vector<std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;
cv::Mat imageClone = normalImage.clone();
cv::Mat dilateElement = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3), cv::Point(1, 1));
cv::dilate(imageClone, imageClone, dilateElement);
findContours(imageClone.clone(), contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
cv::Mat invertedImage = normalImage.clone();
if (contours.size() < 1){
qDebug() << "Error, 0 connected components detected";
}
else {
for (int x = 0; x < normalImage.cols; x++){
for (int y = 0; y < normalImage.rows; y++){
if (imageClone.at<uchar>(y, x) == 255){
invertedImage.at<uchar>(y, x) = 0;
}
else if (pointPolygonTest(contours[0], cv::Point2f(x,y), true) >= 0){
invertedImage.at<uchar>(y, x) = 255;
}
else {
invertedImage.at<uchar>(y, x) = 0;
}
}
}
}
std::vector<std::vector<cv::Point> > invertedContours;
std::vector<cv::Vec4i> invertedHierarchy;
findContours(invertedImage, invertedContours, invertedHierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
for (unsigned int i = 0; i < invertedContours.size(); i++){
cv::Rect rect = minAreaRect(invertedContours[i]).boundingRect();
double xNorm = ((double) rect.x + rect.width/2) / skeletonizedImage.cols;
double yNorm = ((double) rect.y + rect.height/2) / skeletonizedImage.rows;
cv::Point2d point(xNorm, yNorm);
if (rect.width > 2 && rect.height > 2){
listHoles.push_back(point);
}
else {
listFakeHoles.push_back(point);
}
}
// COUNTING JUNCTIONS AND LINE ENDS
for (int x = 0; x < skeletonizedImage.cols; x++){
for (int y = 0; y < skeletonizedImage.rows; y++){
if (skeletonizedImage.at<uchar>(y, x) == 255){
int count = 0;
QVector<cv::Point2i> list;
for (int i = -1; i <= 1; i++){
for (int j = -1; j <= 1; j++){
if (i != 0 || j != 0){
if (y+j >= 0 && y+j < skeletonizedImage.rows && x+i >= 0 && x+i < skeletonizedImage.cols && skeletonizedImage.at<uchar>(y+j, x+i) == 255){
bool neighbourg = false;
cv::Point2i point(x+i,y+j);
for (int k = 0; k < list.size(); k++){
if (cv::norm(point - list[k]) == 1){
neighbourg = true;
}
}
if (!neighbourg){
count++;
list.append(point);
}
}
}
}
}
if (count == 1 || count > 2){
double xNorm = ((double) x) / skeletonizedImage.cols;
double yNorm = ((double) y) / skeletonizedImage.rows;
cv::Point2d point(xNorm,yNorm);
if (count == 1){
listLineEnds.push_back(point);
}
else {
listJunctions.push_back(point);
}
}
}
}
}
// MERGING CLOSE JUNCTIONS
bool done = true;
do {
done = true;
int keepIndexJunction = -1;
int removeIndexJunction = -1;
for (int i = 0; i < listJunctions.size(); i++){
for (int j = 0; j < listJunctions.size(); j++){
if (i != j && norm(listJunctions[i] - listJunctions[j]) < MERGE_DISTANCE){
keepIndexJunction = i;
removeIndexJunction = j;
}
}
}
if (keepIndexJunction != -1 && removeIndexJunction != -1){
done = false;
listJunctions[keepIndexJunction].x = (listJunctions[keepIndexJunction].x + listJunctions[removeIndexJunction].x) / 2;
listJunctions[keepIndexJunction].y = (listJunctions[keepIndexJunction].y + listJunctions[removeIndexJunction].y) / 2;
listJunctions.removeAt(removeIndexJunction);
}
} while (!done);
// DELETING CLOSE JUNCTIONS AND LINE ENDS, ALWAYS WRONG DATA
done = true;
do {
done = true;
int removeIndexLineEnds = -1;
int removeIndexJunctions = -1;
for (int i = 0; i < listLineEnds.size(); i++){
for (int j = 0; j < listJunctions.size(); j++){
if (norm(listLineEnds[i] - listJunctions[j]) < DELETE_DISTANCE){
removeIndexLineEnds = i;
removeIndexJunctions = j;
}
}
}
if (removeIndexLineEnds != -1 && removeIndexJunctions != -1){
done = false;
listLineEnds.removeAt(removeIndexLineEnds);
listJunctions.removeAt(removeIndexJunctions);
}
} while (!done);
// DELETING JUNCTIONS CLOSE TO FAKE LOOPS
done = true;
do {
done = true;
int removeIndexJunction = -1;
for (int i = 0; i < listJunctions.size(); i++){
for (int j = 0; j < listFakeHoles.size(); j++){
if (norm(listJunctions[i] - listFakeHoles[j]) < FAKE_LOOPS_DISTANCE){
removeIndexJunction = i;
}
}
}
if (removeIndexJunction != -1){
done = false;
listJunctions.removeAt(removeIndexJunction);
}
} while (!done);
// DELETING LINE ENDS CLOSE TO FAKE LOOPS
done = true;
do {
done = true;
int removeIndexEnd = -1;
for (int i = 0; i < listLineEnds.size(); i++){
for (int j = 0; j < listFakeHoles.size(); j++){
if (norm(listLineEnds[i] - listFakeHoles[j]) < FAKE_LOOPS_DISTANCE){
removeIndexEnd = i;
}
}
}
if (removeIndexEnd != -1){
done = false;
listLineEnds.removeAt(removeIndexEnd);
}
} while (!done);
// DELETING JUNCTIONS CLOSE TO BORDERS, ALWAYS WRONG DATA
done = true;
do {
done = true;
int removeIndexJunctions = -1;
for (int i = 0; i < listJunctions.size(); i++){
if (listJunctions[i].x < JUNCTION_MARGIN || listJunctions[i].y < JUNCTION_MARGIN || listJunctions[i].x > 1 - JUNCTION_MARGIN || listJunctions[i].y > 1 - JUNCTION_MARGIN){
removeIndexJunctions = i;
}
}
if (removeIndexJunctions != -1){
done = false;
listJunctions.removeAt(removeIndexJunctions);
}
} while (!done);
massCenter = getMassCenter(normalImage);
total = getCount(normalImage);
setParts(normalImage);
listLineEnds = sort(listLineEnds);
listHoles = sort(listHoles);
listJunctions = sort(listJunctions);
}
