static void find_unittest(void)
{
PERFOMANCE_INIT();
PERFOMANCE_TIME("Begin find_unittest");
kstat<7> kstat_instance;
std::string str("AAACCCGGGTTTATATATATATATATATAT");
for(std::string::iterator p = str.begin(); p != str.end() - 7; ++p)
{
kstat_instance.add(p, p + 7, std::distance(str.begin(), p));
}
std::string needle("ACCCGGG");
std::pair<std::set<kstat<7>::link_type>::const_iterator,
std::set<kstat<7>::link_type>::const_iterator> result = kstat_instance.find(needle.begin(), needle.end());
for(std::set<kstat<7>::link_type>::const_iterator i = result.first; i != result.second; ++i)
{
std::cout << *i << ' ';
}
std::cout << std::endl;
std::string needle2("ATATATA");
result = kstat_instance.find(needle2.begin(), needle2.end());
for(std::set<kstat<7>::link_type>::const_iterator i = result.first; i != result.second; ++i)
{
std::cout << *i << ' ';
}
std::cout << std::endl;
PERFOMANCE_TIME("End find_unittest");
}
int EscenarioFile::find_line_starting_in(int pos, const char* chars, bool throw_exception){
std::string needle(chars);
for(unsigned int p = pos; p < lines.size(); p++)
if(Utils::begins_with(lines[p], needle))
return p;
if (throw_exception)
throw std::runtime_error(std::string("File section not found: ") + std::string(chars));
return -1;
}
void tst_QStringMatcher::assignOperator()
{
QString needle("d");
QString hayStack("abcdef");
QStringMatcher m1(needle);
QCOMPARE(m1.indexIn(hayStack), 3);
QStringMatcher m2 = m1;
QCOMPARE(m2.pattern(), needle);
QCOMPARE(m2.indexIn(hayStack), 3);
}
// Match passed string against specified item descriptions and return details for matches
// Element in results array set to zero if their description matches the passed string
//
void List::filter_by_description(Uint8 *storage_items_filter, const ground_item *storage_items, const char *filter_item_text, int no_storage)
{
if (!info_available() || (no_storage<=0) || !storage_items_filter || !storage_items)
return;
std::string needle(filter_item_text);
std::transform(needle.begin(), needle.end(), needle.begin(), tolower);
for (size_t i=0; i<static_cast<size_t>(no_storage); i++)
{
storage_items_filter[i] = 1;
Item *item = get_item(storage_items[i].id, storage_items[i].image_id);
if (item)
{
std::string haystack(item->get_description());
std::transform(haystack.begin(), haystack.end(), haystack.begin(), tolower);
if (haystack.find(needle) != std::string::npos)
storage_items_filter[i] = 0;
}
}
}
void AbstractPlainGdbAdapter::handleInfoTarget(const GdbResponse &response)
{
if (response.resultClass == GdbResultDone) {
// [some leading stdout here]
// >&" Entry point: 0x80831f0 0x08048134 - 0x08048147 is .interp\n"
// [some trailing stdout here]
QString msg = _(response.consoleStreamOutput);
QRegExp needle(_("\\bEntry point: 0x([0-9a-f]+)\\b"));
if (needle.indexIn(msg) != -1) {
m_engine->m_entryPoint =
"0x" + needle.cap(1).toLatin1().rightJustified(sizeof(void *) * 2, '0');
m_engine->postCommand("tbreak *0x" + needle.cap(1).toAscii());
// Do nothing here - inferiorPrepared handles the sequencing.
} else {
m_engine->notifyInferiorSetupFailed(_("Parsing start address failed"));
}
} else if (response.resultClass == GdbResultError) {
m_engine->notifyInferiorSetupFailed(_("Fetching start address failed"));
}
}
void unitTestFSAGenerate(FSA &fsa)
{
std::string needle("needle");
// this function resets State's internal counter for unique state names
State::resetNames();
fsa = FSA::generateFromNeedle(needle);
try {
assert(fsa["S0"]['n'] == "S1");
assert(fsa["S1"]['e'] == "S2");
assert(fsa["S2"]['e'] == "S3");
assert(fsa["S3"]['d'] == "S4");
assert(fsa["S4"]['l'] == "S5");
assert(fsa["S5"]['e'] == "S6");
assert(fsa["S6"].isFinal());
assert(fsa.getInitialState() == "S0");
assert(fsa.getState() == "S0");
} catch (const std::out_of_range &oor) {
std::cerr << oor.what() << std::endl << "FSA improperly generated, aborting..." << std::endl;
abort();
}
std::cout << "FSA generation passed unit tests" << std::endl;
}
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);
}
