int main(int argc, char* argv[]) {
gl::image img;
img.from_pgm("data\\3.pgm");
gl::image bw_img(img.size_x, img.size_y);
threshold_filter(img, bw_img, 10);
gl::recursive_contour center_circle;
center_circle.search(gl::point(img.size_x / 2, img.size_y / 2), &bw_img);
gl::point center = center_circle.mass_center();
gl::point start = center;
while (img.at(start) < 100) start.x += 1;
for (int y = 0; y < img.size_y; ++y)
for (int x = 0; x < img.size_x; ++x) {
int c = img.at(x, y);
assert(c >= 0);
if (c < 60)
img.at(x, y) = 0;
}
img.to_pgm("out.pgm");
return 0;
const double pi = 22.0 / 7.0;
double rad_1 = 2 * pi / 360; // ~0.017
double angle = 0;
double angle_delta = rad_1;
double r = start.x - center.x;
double r_delta = 0.032;
int x = start.x, y = start.y;
int last_x = x, last_y = y;
while (x >= 0 && x < img.size_x && y >= 0 && y < img.size_y) {
x = center.x + r * std::cos(angle);
y = center.y + r * std::sin(angle);
img.draw_line(gl::point(x, y), gl::point(last_x, last_y), 0);
angle += angle_delta;
r += r_delta;
// angle_delta -= 0.005;
// r_delta /= 1.05;
last_x = x;
last_y = y;
}
img.to_pgm("3-1.pgm");
return 0;
}
cv::Mat ArmObjSegmentation::segment(cv::Mat& color_img, cv::Mat& depth_img, cv::Mat& self_mask,
cv::Mat& table_mask, bool init_color_models)
{
cv::Mat color_img_lab_uchar(color_img.size(), color_img.type());
cv::Mat color_img_lab(color_img.size(), CV_32FC3);
cv::cvtColor(color_img, color_img_lab_uchar, CV_BGR2HSV);
// cv::cvtColor(color_img, color_img_lab_uchar, CV_BGR2Lab);
color_img_lab_uchar.convertTo(color_img_lab, CV_32FC3, 1.0/255);
cv::Mat tmp_bw(color_img.size(), CV_8UC1);
cv::Mat bw_img(color_img.size(), CV_32FC1);
// Convert to grayscale
cv::cvtColor(color_img, tmp_bw, CV_BGR2GRAY);
tmp_bw.convertTo(bw_img, CV_32FC1, 1.0/255);
#ifdef VISUALIZE_GRAPH_WEIGHTS
cv::Mat fg_weights(color_img.size(), CV_32FC1, cv::Scalar(0.0));
cv::Mat fg_tied_weights(color_img.size(), CV_32FC1, cv::Scalar(0.0));
cv::Mat bg_tied_weights(color_img.size(), CV_32FC1, cv::Scalar(0.0));
cv::Mat bg_weights(color_img.size(), CV_32FC1, cv::Scalar(0.0));
cv::Mat wu_weights(color_img.size(), CV_32FC1, cv::Scalar(0.0));
cv::Mat wl_weights(color_img.size(), CV_32FC1, cv::Scalar(0.0));
#endif // VISUALIZE_GRAPH_WEIGHTS
// TODO: Clean this up once we have stuff working well
cv::Mat inv_self_mask;
cv::bitwise_not(self_mask, inv_self_mask);
cv::Mat much_larger_mask(self_mask.size(), CV_8UC1, cv::Scalar(255));
cv::Mat enlarge_element(bg_enlarge_size_, bg_enlarge_size_, CV_8UC1, cv::Scalar(255));
cv::dilate(inv_self_mask, much_larger_mask, enlarge_element);
cv::Mat larger_mask, known_arm_mask;
cv::Mat arm_band = getArmBand(inv_self_mask, arm_enlarge_width_, arm_shrink_width_, false,
larger_mask, known_arm_mask);
// Get known arm pixels
cv::Mat known_arm_pixels;
color_img_lab.copyTo(known_arm_pixels, known_arm_mask);
cv::Mat known_bg_mask = much_larger_mask - larger_mask;
// Get known object pixels
cv::Mat known_bg_pixels;
color_img_lab.copyTo(known_bg_pixels, known_bg_mask);
// Get stats for building graph
int num_nodes = 0;
int num_edges = 0;
tabletop_pushing::NodeTable nt;
for (int r = 0; r < much_larger_mask.rows; ++r)
{
for (int c = 0; c < much_larger_mask.cols; ++c)
{
if (much_larger_mask.at<uchar>(r,c) > 0)
{
// Add this as another node
int cur_idx = num_nodes++;
nt.addNode(r, c, cur_idx);
int test_idx = nt.getIdx(r,c);
// Check for edges to add
// left edge
if (c > 0 && much_larger_mask.at<uchar>(r, c-1) > 0)
{
num_edges++;
}
if (r > 0)
{
// Up edge
if(much_larger_mask.at<uchar>(r-1,c) > 0)
{
num_edges++;
}
}
}
}
}
if (num_nodes < 1)
{
cv::Mat empty(color_img.size(), CV_8UC1, cv::Scalar(0));
return empty;
}
if (!have_arm_color_model_)
{
std::cout << "[arm_obj_segmentation]: Building arm color model." << std::endl;
arm_color_model_ = getGMMColorModel(known_arm_pixels, known_arm_mask, 3);
have_arm_color_model_ = true;
}
if(!have_bg_color_model_)
{
std::cout << "[arm_obj_segmentation]: Building bg color model." << std::endl;
bg_color_model_ = getGMMColorModel(known_bg_pixels, known_bg_mask, 5);
have_bg_color_model_ = true;
}
#ifdef USE_CANNY_EDGES
cv::Mat canny_edges_8bit;
cv::Mat canny_edges;
cv::Canny(tmp_bw, canny_edges_8bit, 120, 250);
canny_edges_8bit.convertTo(canny_edges, CV_32FC1, 1.0/255);
#else
cv::Mat Ix = getXImageDeriv(bw_img);
cv::Mat Iy = getYImageDeriv(bw_img);
#endif
cv::Mat Dx = getXImageDeriv(depth_img);
cv::Mat Dy = getYImageDeriv(depth_img);
tabletop_pushing::GraphType *g;
g = new GraphType(num_nodes, num_edges);
// Populate unary and binary edge weights
for (int r = 0; r < much_larger_mask.rows; ++r)
{
for (int c = 0; c < much_larger_mask.cols; ++c)
{
if (much_larger_mask.at<uchar>(r,c) > 0)
{
int cur_idx = nt.getIdx(r,c);
float fg_weight = 0.0;
float bg_weight = 0.0;
if (known_arm_mask.at<uchar>(r,c) > 0)
{
fg_weight = fg_tied_weight_;
bg_weight = 0.0;
}
else if (known_bg_mask.at<uchar>(r,c) > 0 || table_mask.at<uchar>(r,c) > 0)
{
fg_weight = 0.0;
bg_weight = bg_tied_weight_;
}
else
{
fg_weight = getUnaryWeight(color_img_lab.at<cv::Vec3f>(r,c), arm_color_model_);
bg_weight = getUnaryWeight(color_img_lab.at<cv::Vec3f>(r,c), bg_color_model_)*0.5;
}
g->add_node();
g->add_tweights(cur_idx, fg_weight, bg_weight);
#ifdef VISUALIZE_GRAPH_WEIGHTS
fg_weights.at<float>(r,c) = fg_weight;
bg_weights.at<float>(r,c) = bg_weight;
#endif // VISUALIZE_GRAPH_WEIGHTS
// Add left link
if (c > 0 && much_larger_mask.at<uchar>(r, c-1) > 0)
{
int other_idx = nt.getIdx(r, c-1);
#ifdef USE_CANNY_EDGES
float w_l = getEdgeWeightBoundary(canny_edges.at<float>(r,c), Dx.at<float>(r,c),
canny_edges.at<float>(r, c-1), Dx.at<float>(r, c-1));
#else // USE_CANNY_EDGES
float w_l = getEdgeWeightBoundary(Ix.at<float>(r,c), Dx.at<float>(r,c),
Ix.at<float>(r, c-1), Dx.at<float>(r, c-1));
#endif // USE_CANNY_EDGES
g->add_edge(cur_idx, other_idx, /*capacities*/ w_l, w_l);
#ifdef VISUALIZE_GRAPH_WEIGHTS
wl_weights.at<float>(r,c) = w_l;
#endif // VISUALIZE_GRAPH_WEIGHTS
}
if (r > 0)
{
// Add up link
if(much_larger_mask.at<uchar>(r-1,c) > 0)
{
int other_idx = nt.getIdx(r-1, c);
#ifdef USE_CANNY_EDGES
float w_u = getEdgeWeightBoundary(canny_edges.at<float>(r, c), Dy.at<float>(r, c),
canny_edges.at<float>(r-1, c), Dy.at<float>(r-1, c));
#else // USE_CANNY_EDGES
float w_u = getEdgeWeightBoundary(Iy.at<float>(r,c), Dx.at<float>(r,c),
Iy.at<float>(r, c-1), Dx.at<float>(r, c-1));
#endif // USE_CANNY_EDGES
g->add_edge(cur_idx, other_idx, /*capacities*/ w_u, w_u);
#ifdef VISUALIZE_GRAPH_WEIGHTS
wu_weights.at<float>(r,c) = w_u;
#endif // VISUALIZE_GRAPH_WEIGHTS
}
}
}
}
}
// Perform cut
g->maxflow(false);
// Convert output into image
cv::Mat segs = convertFlowToMat(g, nt, color_img.rows, color_img.cols);
// Cleanup
delete g;
cv::Mat graph_input;
cv::Mat segment_arm;
cv::Mat segment_bg;
color_img.copyTo(graph_input, much_larger_mask);
color_img.copyTo(segment_arm, segs);
color_img.copyTo(segment_bg, (segs == 0));
//cv::imshow("segments", segs);
// cv::imshow("Graph input", graph_input);
cv::imshow("Arm segment", segment_arm);
cv::imshow("Background segment", segment_bg);
cv::imshow("Table mask", table_mask);
// cv::imshow("Known bg mask", known_bg_mask);
#ifdef VISUALIZE_GRAPH_WEIGHTS
double min_val, max_val;
cv::minMaxLoc(fg_weights, &min_val, &max_val);
cv::imshow("fg weights", (fg_weights-min_val)/(max_val-min_val));
// std::cout << "Max fg weight: " << max_val << std::endl;
// std::cout << "Min fg weight: " << min_val << std::endl;
cv::minMaxLoc(bg_weights, &min_val, &max_val);
cv::imshow("bg weights", (bg_weights-min_val)/(max_val-min_val));
// std::cout << "Max bg weight: " << max_val << std::endl;
// std::cout << "Min bg weight: " << min_val << std::endl;
// cv::minMaxLoc(wu_weights, &min_val, &max_val);
// cv::imshow("up weights", wu_weights/max_val);
// std::cout << "Max up weight: " << max_val << std::endl;
// std::cout << "Min up weight: " << min_val << std::endl;
// cv::minMaxLoc(wl_weights, &min_val, &max_val);
// cv::imshow("left weights", wl_weights/max_val);
// std::cout << "Max left weight: " << max_val << std::endl;
// std::cout << "Min left weight: " << min_val << std::endl;
#endif // VISUALIZE_GRAPH_WEIGHTS
#ifdef USE_CANNY_EDGES
cv::imshow("Canny", canny_edges);
#endif
return segs;
}