static getRobotX(Info * info) {
int ret = 0;
int x1 = info->video_info->robot_acc_pos.x;
int x2 = info->video_info2->robot_acc_pos.x;
if (x_bound(x1) && x_bound(x2) && info->video_info->is_robot_seen && info->video_info2->is_robot_seen) ret = (x1 + x2) / 2;
else if (x_bound(x1) && info->video_info->is_robot_seen) ret = x1;
else if (x_bound(x2) && info->video_info2->is_robot_seen) ret = x2;
return ret;
}
static int getDoorRightx(Info * info) {
int ret = 0;
int x1 = info->video_info->door_right.x;
int x2 = info->video_info2->door_right.x;
if (x_bound(x1) && x_bound(x2) && info->video_info->is_door_right && info->video_info2->is_door_right) ret = (x1 + x2) / 2;
else if (x_bound(x1) && info->video_info->is_door_right) ret = x1;
else if (x_bound(x2) && info->video_info2->is_door_right) ret = x2;
return ret;
}
static int getx(Info * info) {
int ret = 0;
int x1 = info->video_info->ball_acc_pos.x;
int x2 = info->video_info2->ball_acc_pos.x;
if (x_bound(x1) && x_bound(x2) && info->video_info->is_ball_seen && info->video_info2->is_ball_seen) ret = (x1 + x2) / 2;
else if (x_bound(x1) && info->video_info->is_ball_seen) ret = x1;
else if (x_bound(x2) && info->video_info2->is_ball_seen) ret = x2;
return ret;
}
void MultisphereParallel::writeRestart(FILE *fp)
{
double *sendbuf = 0, *recvbuf = 0;
double xbnd[3];
bool dummy = false;
double nba = static_cast<double>(n_body_all());
int sizeLocal = n_body() * (customValues_.elemBufSize(OPERATION_RESTART,dummy,dummy,dummy) + 4);
int sizeGlobal = 0, sizeOne = 0;
// allocate send buffer and pack element data
// all local elements are in list
memory->create(sendbuf,sizeLocal,"MultiNodeMeshParallel::writeRestart:sendbuf");
sizeLocal = 0;
for(int i = 0; i < n_body(); i++)
{
x_bound(xbnd,i);
sizeOne = customValues_.pushElemToBuffer(i,&(sendbuf[sizeLocal+4]),OPERATION_RESTART,dummy,dummy,dummy);
sendbuf[sizeLocal] = static_cast<double>(sizeOne+4);
sendbuf[sizeLocal+1] = xbnd[0];
sendbuf[sizeLocal+2] = xbnd[1];
sendbuf[sizeLocal+3] = xbnd[2];
sizeLocal += (sizeOne+4);
}
// gather the per-element data
sizeGlobal = MPI_Gather0_Vector(sendbuf,sizeLocal,recvbuf,world);
// write data to file
if(comm->me == 0)
{
// size with 1 extra value (nba)
int size = (1+sizeGlobal) * sizeof(double);
// write size
fwrite(&size,sizeof(int),1,fp);
// write extra value
fwrite(&nba,sizeof(double),1,fp);
// write per-element data
fwrite(recvbuf,sizeof(double),sizeGlobal,fp);
}
// clean up
memory->destroy(sendbuf);
if(recvbuf)
delete []recvbuf;
}
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);
}
