static int gety(Info * info) { int ret = 0; int y1 = info->video_info->ball_acc_pos.y; int y2 = info->video_info2->ball_acc_pos.y; if (y_bound(y1) && y_bound(y2) && info->video_info->is_ball_seen && info->video_info2->is_ball_seen && info->video_info->is_ball_acc && info->video_info2->is_ball_acc) ret = (y1 + y2) / 2; else if (y_bound(y1) && info->video_info->is_ball_seen && info->video_info->is_ball_acc) ret = y1; else if (y_bound(y2) && info->video_info2->is_ball_seen && info->video_info2->is_ball_acc) ret = y2; return ret; }
static getRobotY(Info * info) { int ret = 0; int y1 = info->video_info->robot_acc_pos.y; int y2 = info->video_info2->robot_acc_pos.y; if (y_bound(y1) && y_bound(y2) && info->video_info->is_robot_seen && info->video_info2->is_robot_seen) ret = (y1 + y2) / 2; else if (y_bound(y1) && info->video_info->is_robot_seen) ret = y1; else if (y_bound(y2) && info->video_info2->is_robot_seen) ret = y2; return ret; }
static int getDoorRighty(Info * info) { int ret = 0; int y1 = info->video_info->door_right.y; int y2 = info->video_info2->door_right.y; if (y_bound(y1) && y_bound(y2) && info->video_info->is_door_right && info->video_info2->is_door_right) ret = (y1 + y2) / 2; else if (y_bound(y1) && info->video_info->is_door_right) ret = y1; else if (y_bound(y2) && info->video_info2->is_door_right) ret = y2; return ret; }
static int getEdgeX(Info * info) { int ret = 0; int x1 = info->video_info->edge_info.x; int x2 = info->video_info2->edge_info.x; if (y_bound(x1) && y_bound(x2) && info->video_info->is_edge_valid && info->video_info2->is_edge_valid) return min(x1, x2); else if (y_bound(x1) && info->video_info->is_edge_valid) return x1; else if (y_bound(x2) && info->video_info2->is_edge_valid) return x2; else return 0; }
static int getDoorMidy(Info * info) { int ret = 0; int y1 = info->video_info->door_acc_pos.y; int y2 = info->video_info2->door_acc_pos.y; if (y_bound(y1) && y_bound(y2) && info->video_info->is_door_seen && info->video_info2->is_door_seen) ret = (y1 + y2) / 2; else if (y_bound(y1) && info->video_info->is_door_seen) ret = y1; else if (y_bound(y2) && info->video_info2->is_door_seen) ret = y2; return ret; }
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); }