void computeSpatiogram(const cv::Mat &imagePatch, int m, spatiogram &sg){
int n = 256/m;
std::vector<double> bins;
linspace(bins, 0, 256, n);
int height = imagePatch.rows;
int width = imagePatch.cols;
cv::Mat X,Y, Xm, Ym, dist2, K;
meshgrid(cv::Range(1,width), cv::Range(1,height), X, Y);
meshgrid(cv::Range(-width/2,width/2), cv::Range(-height/2,height/2), Xm, Ym);
X.convertTo(X, CV_64FC1); Y.convertTo(Y, CV_64FC1);
robustnorm(Xm,Ym,dist2);
dist2 = dist2(cv::Rect(0,0,width,height));
epanechnikov(dist2, K);
// initialize spatiogram
sg.C = 1/cv::sum(K)[0];
sg.C = sg.C/(imagePatch.channels());
sg.cd = cv::Mat::zeros(1, m*imagePatch.channels(), CV_64FC1);
sg.mu = cv::Mat::zeros(2, m*imagePatch.channels(), CV_64FC1);
sg.cm = cv::Mat::zeros(2, m*imagePatch.channels(), CV_64FC1);
sg.bins = m*imagePatch.channels();
for (int l=0;l<imagePatch.channels();l++){
for (int j=0;j<m;j++){
cv::Mat temp;
binelements(imagePatch, bins, l, j, temp);
// zeroth order spatiogram
sg.cd.at<double>(0,l*m+j) = cv::sum( K.mul(temp) )[0];
double den = cv::sum(temp)[0];
if (den==0.0) den=std::numeric_limits<double>::epsilon();
if (sg.cd.at<double>(0,l*m+j)==0.0) sg.cd.at<double>(0,l*m+j)=std::numeric_limits<double>::epsilon();
// Mean vector of the pixels' coordinates mu: first order spatiogram
double mu_x = cv::sum( X.mul(temp) )[0]/den;
double mu_y = cv::sum( Y.mul(temp) )[0]/den;
sg.mu.at<double>(0, l*m+j) = mu_x;
sg.mu.at<double>(1, l*m+j) = mu_y;
// Covariance matrix of the pixels' coordinates Cm[2x2]: 2nd order spatiogram
cv::Mat C11; cv::pow(X - mu_x, 2, C11); //(x-mu_x)^2
cv::Mat C22; cv::pow(Y - mu_y, 2, C22); //(y-mu_y)^2
sg.cm.at<double>(0, l*m+j) = cv::sum(C11.mul(temp))[0]/den; //Cov(1,1)
sg.cm.at<double>(1, l*m+j) = cv::sum(C22.mul(temp))[0]/den; //Cov(2,2)
}
}
//normalize color distribution
sg.cd = sg.C*sg.cd;
}
cv::Point2d computeMeanshiftVector(const cv::Rect &patch, const cv::Mat &weights, const cv::Mat &v){
int height = patch.height;
int width = patch.width;
cv::Mat X,Y;
meshgrid(cv::Range(1,width), cv::Range(1,height), X, Y);
X.convertTo(X, CV_64FC1);
Y.convertTo(Y, CV_64FC1);
double den = cv::sum(weights)[0]+std::numeric_limits<double>::epsilon();
cv::Point2d z;
z.x = ( cv::sum(X.mul(weights))[0] - cv::sum(v.row(0))[0] )/den;
z.y = ( cv::sum(Y.mul(weights))[0] - cv::sum(v.row(1))[0] )/den;
z.x = patch.x-patch.width/2 + z.x;
z.y = patch.y-patch.height/2 + z.y;
return z;
}
Mat matlabHelper::gaussianKernal(double sigma, int size)
{
Mat K, x, y;
double siz = (size-1)/2;
Range xrg, yrg;
xrg.start = -siz; xrg.end = siz;
yrg.start = -siz; yrg.end = siz;
double std = sigma;
meshgrid(xrg, yrg, x, y);
Mat tmp1,tmp2,tmp3;
cv::multiply(x, x, tmp1);
cv::multiply(y, y, tmp2);
cv::add(tmp1,tmp2,tmp3);
tmp3.convertTo(tmp3, CV_64FC1);
Mat arg(size, size, CV_64FC1);
cv::divide( tmp3, (double)(-2*std*std), arg);
Mat h(size, size, CV_64FC1);
double sumh = 0;
for(int i = 0; i < arg.rows; i++)
{
for(int j = 0; j < arg.cols; j++)
{
h.at<double>(i,j) = exp(arg.at<double>(i,j));
sumh += h.at<double>(i,j);
}
}
if( sumh != 0)
{
for(int i = 0; i < arg.rows; i++)
{
for(int j = 0; j < arg.cols; j++)
{
h.at<double>(i,j) = h.at<double>(i,j)/sumh;
}
}
}
return h;
}
template <class T> inline
array_2d<T> meshgrid(const index_array& a)
{
return meshgrid(a, a);
}
/* Run Program */
int main (int argc, char** argv) {
//Read input argument
int steps=1000;
double h = 0.1;
if (argc == 1){
printf("USAGE: NumberOfSteps StepSize/Tolerance\nExited.\n");
return 0;
}
if (argc > 1) steps = atoi(argv[1]);
if (argc > 2) h = atof(argv[2]);
printf("Input arguments: %i %f \n", steps, h);
//Check if SDL Initialization is successful
if (SDL_Init(SDL_INIT_VIDEO) != 0) {
return -1;
}
//Create Window
SDL_Window *win = SDL_CreateWindow("Exercise 5, Lorenz Equations",50,50,800,600,SDL_WINDOW_SHOWN | SDL_WINDOW_OPENGL);
if (!win) {
fprintf(stderr, "Could not create window: %s\n", SDL_GetError());
}
//Initialize OpenGL
SDL_GLContext context = SDL_GL_CreateContext(win);
if (!context) {
fprintf(stderr, "Could not create OpenGL context: %s\n", SDL_GetError());
}
glViewport(0,0,(GLsizei)800,(GLsizei)600);
glClearColor(1.0f,1.0f,1.0f,1.0f);
glClear(GL_COLOR_BUFFER_BIT);
SDL_GL_SwapWindow(win);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glEnable(GL_DEPTH_TEST);
glFrustum(-100,100,-100,100,0.01,100);
glEnable(GL_POLYGON_OFFSET_FILL);
glPolygonOffset(1,1);
glEnable(GL_LIGHTING);
glShadeModel(GL_SMOOTH);
GLfloat lightcol[] = {0.9,0.9,0.9,1.0};
GLfloat lightamb[] = {0.5,0.5,0.5,1.0};
GLfloat lightpos[] = {20.0,20.0,30.0,1.0};
glLightfv(GL_LIGHT0,GL_AMBIENT,lightamb);
glLightfv(GL_LIGHT0,GL_DIFFUSE,lightcol);
glEnable(GL_LIGHT0);
glEnable(GL_COLOR_MATERIAL);
glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
glEnable(GL_RESCALE_NORMAL);
//Init all vectors
double t0 = 0;
double* t = malloc(sizeof(double)*steps);
double* xValues = malloc(sizeof(double)*steps);
double* yValues = malloc(sizeof(double)*steps);
double* zValues = malloc(sizeof(double)*steps);
double* xValues_it = malloc(sizeof(double)*steps);
double* yValues_it = malloc(sizeof(double)*steps);
double* zValues_it = malloc(sizeof(double)*steps);
double xmin = -4;
double xmax = 4;
double ymin = -4;
double ymax = 4;
double xsteps = 21;
double ysteps = 21;
struct Matrix* X = new_Matrix(xsteps,ysteps);
struct Matrix* Y = new_Matrix(xsteps,ysteps);
struct Matrix* Z = new_Matrix(xsteps,ysteps);
meshgrid(xmin,xmax,ymin,ymax,xsteps,ysteps,X,Y);
paraboloid(X,Y,Z);
struct Vector* vec0 = new_Vector(DIM);
vec0->values[0] = 1;
vec0->values[1] = 1;
vec0->values[2] = 2;
vec0->values[3] = 1-0.25;
vec0->values[4] = -1-0.25;
vec0->values[5] = -1;
struct Vector** vec = malloc(sizeof(struct Vector*)*steps);
for (int j=1; j < steps; j++) {
vec[j] = new_Vector(DIM);
}
//find geodesic iteratively
struct Vector** vec_it = malloc(sizeof(struct Vector*)*steps);
vec_it[0] = vec0;
for (int j=1; j < steps; j++) {
vec_it[j] = new_Vector(DIM);
geodesic_step(vec_it[j-1],h,vec_it[j]);
}
for (int j=0; j < steps; j++) {
xValues_it[j] = vec_it[j]->values[0];
yValues_it[j] = vec_it[j]->values[1];
zValues_it[j] = vec_it[j]->values[2];
}
//solve ODE
//adaptive_rk3(geodesic_rhs,t0,vec0,h,t,vec,steps);
euler(geodesic_rhs,t0,vec0,h,t,vec,steps);
for (int j=0; j < steps; j++) {
xValues[j] = vec[j]->values[0];
yValues[j] = vec[j]->values[1];
zValues[j] = vec[j]->values[2];
}
SDL_GL_SwapWindow(win);
/* Look Around Loop */
bool loop = true;
bool wireframe = false;
bool overlay = true;
double xMouse = 0;
double yMouse = 0;
double xpos = 0;
double ypos = 0;
double zpos = 0;
bool mouseDown = false;
double mouseZoom = 0.1;
SDL_Event event;
while (loop) {
while (SDL_PollEvent(&event)) {
if (event.type == SDL_QUIT) {
loop = false;
}
if (event.type== SDL_MOUSEBUTTONDOWN) {
mouseDown = true;
}
if (event.type == SDL_MOUSEBUTTONUP) {
mouseDown = false;
}
if (event.type == SDL_MOUSEMOTION) {
if(mouseDown){
xMouse += 0.5*event.motion.xrel;
yMouse += 0.5*event.motion.yrel;
}
}
if (event.type== SDL_KEYDOWN) {
if(event.key.keysym.scancode == SDL_SCANCODE_W)
ypos += 0.1;
if(event.key.keysym.scancode == SDL_SCANCODE_S)
ypos -= 0.1;
if(event.key.keysym.scancode == SDL_SCANCODE_A)
xpos -= 0.1;
if(event.key.keysym.scancode == SDL_SCANCODE_D)
xpos += 0.1;
if(event.key.keysym.scancode == SDL_SCANCODE_Q)
zpos += 0.1;
if(event.key.keysym.scancode == SDL_SCANCODE_E)
zpos -= 0.1;
if(event.key.keysym.scancode == SDL_SCANCODE_TAB)
wireframe = !wireframe;
if(event.key.keysym.scancode == SDL_SCANCODE_O)
overlay = !overlay;
}
if (event.type== SDL_MOUSEWHEEL) {
if (event.wheel.y < 0)
mouseZoom /= 1.2;
else
mouseZoom *= 1.2;
}
}
//Draw current frame buffer
SDL_GL_SwapWindow(win);
//Prepare next frame buffer
glClearColor( 1.0f, 1.0f, 1.0f, 1.0f );
glClear( GL_COLOR_BUFFER_BIT);
glEnable(GL_DEPTH_TEST);
glClear(GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glTranslated(xpos, ypos, zpos);
glScaled(mouseZoom,mouseZoom,mouseZoom);
glRotated(-90, 1.0, 0.0, 0.0);
glRotated(-yMouse, 1.0, 0.0, 0.0);
glRotated(-xMouse, 0.0, 0.0, 1.0);
plotAxis3D(-10,10,-10,10,-5,30,0,0,0);
glColor4f(0.2,0.4,0.5,1.0);
glLightfv(GL_LIGHT0,GL_POSITION,lightpos);
if (wireframe) {
glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
surf(X,Y,Z);
} else {
glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
surf(X,Y,Z);
if (overlay) {
glColor4f(0.2,0.2,0.2,1.0);
glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
surf(X,Y,Z);
}
}
glColor4f(0.85,0.85,0.10,1.0);
plotArray3D(xValues,yValues,zValues,steps);
glColor4f(0.9,0.2,0.4,1.0);
plotArray3D(xValues_it,yValues_it,zValues_it,steps);
glColor4f(0.0,0.0,0.0,1.0);
}
//Clean Up Memory
free(t);
for (int j=1; j < steps; j++) {
delete_Vector(vec[j]);
}
free(vec);
free(xValues);
free(yValues);
free(zValues);
delete_Vector(vec0);
delete_Matrix(X);
delete_Matrix(Y);
delete_Matrix(Z);
SDL_GL_DeleteContext(context);
SDL_DestroyWindow(win);
SDL_Quit();
return 0;
}
cv::Mat RidgeFilter::run(cv::Mat inputImage, cv::Mat orientationImage, cv::Mat frequency, double kx, double ky)
{
angleInc = 3; // fixed angle increment between filter orientations in degrees.
inputImage.convertTo(inputImage, CV_32FC1);
int rows = inputImage.rows;
int cols = inputImage.cols;
orientationImage.convertTo(orientationImage, CV_32FC1);
enhancedImage = cv::Mat::zeros(rows, cols, CV_32FC1);
cv::vector<int> validr;
cv::vector<int> validc;
int ind = 1;
double freq = frequency.at<float>(1, 1);
double unfreq = freq;
cv::Mat freqindex = cv::Mat::ones(100, 1, CV_32FC1);
double sigmax = (1 / freq) * kx;
double sigmay = (1 / freq) * ky;
int szek = round(3 * (std::max(sigmax, sigmay)));
meshgrid(szek);
reffilter = cv::Mat::zeros(meshX.rows, meshX.cols, CV_32FC1);
meshX.convertTo(meshX, CV_32FC1);
meshY.convertTo(meshY, CV_32FC1);
for (int i = 0; i < meshX.rows; i++)
{
for (int j = 0; j < meshX.cols; j++)
{
float pixVal2 = -0.5*(meshX.at<float>(i, j)*meshX.at<float>(i, j) / (sigmax*sigmax) +
meshY.at<float>(i, j)*meshY.at<float>(i, j) / (sigmay*sigmay));
float pixVal = std::exp(pixVal2);
float cosVal = 2 * pi * unfreq * meshX.at<float>(i, j);
reffilter.at<float>(i, j) = pixVal * cos(cosVal);
}
}
for (int m = 0; m < 180 / angleInc; m++)
{
double angle = -(m*angleInc + 90);
cv::Mat rot_mat = cv::getRotationMatrix2D(cv::Point((float)(reffilter.rows / 2.0F), (float)(reffilter.cols / 2.0F)), angle, 1.0);
cv::Mat rotResult;
cv::warpAffine(reffilter, rotResult, rot_mat, reffilter.size());
filter.push_back(rotResult);
}
// Find indices of matrix points greater than maxsze from the image boundary
int maxsze = szek;
// Convert orientation matrix values from radians to an index value that corresponds
// to round(degrees/angleInc)
int maxorientindex = std::round(180 / angleInc);
cv::Mat orientindex(rows, cols, CV_32FC1);
for (int y = 0; y < rows; y++)
{
for (int x = 0; x < cols; x++)
{
if (x > maxsze && x < rows - maxsze && y > maxsze && y < cols - maxsze)
{
validr.push_back(y);
validc.push_back(x);
}
int orientpix = static_cast<int>(std::round(orientationImage.at<float>(y, x) / pi * 180 / angleInc));
if (orientpix < 0)
{
orientpix += maxorientindex;
}
if (orientpix >= maxorientindex)
{
orientpix -= maxorientindex;
}
orientindex.at<float>(y, x) = orientpix;
}
}
// Finally, do the filtering
for (int k = 0; k < validr.size(); k++)
{
int s = szek;
int r = validr[k];
int c = validc[k];
int filterindex = freqindex.at<float>(std::round(frequency.at<float>(r, c) * 100));
cv::Rect roi(c - s - 1, r - s - 1, meshX.cols, meshX.rows);
cv::Mat subim(meshX.rows, meshX.cols, CV_32FC1);
for (int m = r - s; m < r + s; m++)
{
for (int n = c - s; n < c + s; n++)
{
int tmpRow = m - r + s;
int tmpCol = n - c + s;
if (tmpRow < subim.rows && tmpCol < subim.cols && m < inputImage.rows && n < inputImage.cols)
{
subim.at<float>(m - r + s, n - c + s) = inputImage.at<float>(m, n);
}
}
}
cv::Mat subFilter = filter.at(orientindex.at<float>(r, c));
cv::Mat mulResult;
cv::multiply(subim, subFilter, mulResult);
cv::Scalar resultSum = cv::sum(mulResult);
float value = resultSum[0];
if (value > 0)
{
enhancedImage.at<float>(r, c) = 255;
}
}
cv::Mat aux = enhancedImage.rowRange(0, rows).colRange(0, szek + 1);
aux.setTo(255);
aux = enhancedImage.rowRange(0, szek + 1).colRange(0, cols);
aux.setTo(255);
aux = enhancedImage.rowRange(rows - szek, rows).colRange(0, cols);
aux.setTo(255);
aux = enhancedImage.rowRange(0, rows).colRange(cols - 2 * (szek + 1) - 1, cols);
aux.setTo(255);
return enhancedImage;
}
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);
}
/* Función que encapsula los cálculos e impresiones del flujo complejo en cilindro y perfil */
int flujo(float * dperfil, float * opc, float * opp, float * opf)
{
int i;
char opcionf, opcion;
int mct;
// Vector con los datos del flujo
float * dflujo;
dflujo = (float *) malloc(5 * sizeof(float)); // Reserva de memoria para el vector
// Matriz Nx2 que almacenará los puntos de la circunferencia
float ** circunferencia;
circunferencia = (float **) malloc(N * sizeof(float *)); // Reserva de memoria para cada vector
for (i=0; i < N; i++)
{
circunferencia[i] = (float *) malloc(2 * sizeof(float)); // Reserva de memoria para las dos coordenadas del vector
}
printf("\033[32m \n1. Obtención del flujo a traves del cilindro (Th. Kutta-Yukovski)\n2. Obtención del flujo a traves del perfil alar");
printf("\033[31m (abandona el programa)");
printf("\033[0m \n");
do
{
scanf ("%d", &mct);
} while((mct != 1) && (mct != 2));
// Datos del cilindro
if (dperfil[2]==0) // Si no se han dado los datos del cilindro el radio sera 0
{
do
{
datos_perfil(dperfil);
} while (limites_imaginario(dperfil) == 0);
}
else
{
printf("\033[32m \n¿Desea dar nuevos valores al cilindro? (s/n)");
printf("\033[0m \n");
scanf ("%c", &opcionf);
do
{
scanf ("%c", &opcion);
} while (opcion != 's' && opcion != 'n');
if (opcion == 's')
{
do
{
datos_perfil(dperfil);
} while (limites_imaginario(dperfil) == 0);
}
}
if (mct == 1)
{
// Datos para el flujo
datos_flujo(dperfil, dflujo);
// Calcula los puntos de la circunferencia
matriz_circunferencia(dperfil, circunferencia);
// Guarda los puntos de la circunferencia en el archivo pts_circunferencia.dat
if (imprimir_circunferencia(circunferencia)) // Si la apertura falla vuelve al menú
menu(0, dperfil, opc, opp, opf);
// Array para las coordenadas x de la malla
float ** xx;
xx = (float **) malloc(M * sizeof(float *));
for (i=0; i < M; i++)
xx[i] = (float *) malloc(M * sizeof(float));
// Array para las coordendas y de la malla
float ** yy;
yy = (float **) malloc(M * sizeof(float *));
for (i=0; i < M; i++)
yy[i] = (float *) malloc(M * sizeof(float));
// Array para el flujo en cada punto de la malla
float ** psi;
psi = (float **) malloc(M * sizeof(float *));
for (i=0; i < M; i++)
psi[i] = (float *) malloc(M * sizeof(float));
// Crea la malla en la que calcularemos el flujo
meshgrid(xx, yy, dperfil);
// Calcula el flujo en cada punto de la malla para el cilindro
calculo_flujo(dperfil, dflujo, xx, yy, psi);
// Exporta puntos para imprimir el flujo en el cilindro con GNU Plot
imprimir_flujo(xx, yy, psi);
if (dflujo[3] != 0)
{
printf("\n\033[32mFlujo: ");
printf("\033[0m %f\n", fabsf(dflujo[3]));
}
printf("\033[32mCoeficiente de sustentación:");
printf("\033[0m %f\n", fabsf(dflujo[4]));
plotfc(opf, psi);
}
/* CAMBIAN */
if (mct == 2)
{
int i;
float ** xxpr;
xxpr = (float **) malloc(Mi * sizeof(float *));
for (i=0; i < Mi; i++)
xxpr[i] = (float *) malloc(Mi * sizeof(float));
float ** yypr;
yypr = (float **) malloc(Mi * sizeof(float *));
for (i=0; i < Mi; i++)
yypr[i] = (float *) malloc(Mi * sizeof(float));
complex double ** tt;
tt = (complex double **) malloc(Mi * sizeof(complex double *));
for (i=0; i < Mi; i++)
tt[i] = (complex double *) malloc(Mi * sizeof(complex double));
double ** psipr;
psipr = (double **) malloc(Mi * sizeof(double *));
for (i=0; i < Mi; i++)
psipr[i] = (double *) malloc(Mi * sizeof(double));
float ** xxtau;
xxtau = (float **) malloc(Mi * sizeof(float *));
for (i=0; i < Mi; i++)
xxtau[i] = (float *) malloc(Mi * sizeof(float));
float ** yytau;
yytau = (float **) malloc(Mi * sizeof(float *));
for (i=0; i < Mi; i++)
yytau[i] = (float *) malloc(Mi * sizeof(float));
perfil_imaginario(dperfil);
dflujo[0] = 1;
dflujo[1] = 0.1;
dflujo[2] = 1.00;
dflujo[3] = 4 * M_PI * dperfil[4] * dflujo[0] * (dperfil[1] + (1+dperfil[0]) * dflujo[1]);
dflujo[4] = 2*M_PI*sin(dflujo[1]+dperfil[3]);
meshgrid_imaginario(xxpr, yypr, tt, dperfil);
calculo_flujo_imaginario(tt, psipr, dperfil);
arregla_malla(tt, dperfil);
nueva_malla(xxtau, yytau, tt, dperfil);
imprimir_flujo_perfil(xxtau, yytau, psipr);
if (dflujo[3] != 0)
{
printf("\n\033[32mFlujo: ");
printf("\033[0m %f\n", fabsf(dflujo[3]));
}
printf("\033[32mCoeficiente de sustentación:");
printf("\033[0m %f\n", fabsf(dflujo[4]));
printf("\033[32m************************************************************************\n");
printf("\033[0m\n");
if (plotfp(psipr, opf)==1)
return 1;
}
return 0;
}
void createGabor(double *** &G,int ore[], int n){
int Nscales = sizeof(ore);
int Nfilters = ssum(ore);
int l=0;
matrix<double> param(Nfilters,4);
matrix<int> fx(n,n),fy(n,n);
for (int i=0;i<Nscales-1;i++){
for (int j=1;j<=ore[i];j++){
param.set(l,0,0.35);
param.set(l,1,0.3/pow(1.85,i));
param.set(l,2,16*pow((double)ore[i],2)/pow(32.0,2));
param.set(l,3,(j-1)*PI/(ore[i]));
l=l+1;
}
}
// Frequencies:
meshgrid(fx,fy,-n/2,n/2-1);
matrix<double> fr(n,n),t(n,n),tr(n,n);
fr.squaresqrt(fx,fy);
fr.fftshift();
t.angle(fx,fy);
t.fftshift();
//% Transfer functions:
//G=zeros([n n Nfilters]);
double dtmp;
G = new double **[n];
for(int i=0;i<n;i++){
G[i] = new double*[n];
for (int j=0;j<n;j++)
G[i][j] = new double[Nfilters];
}
for (int i=0;i<Nfilters;i++){
//par=param(i,:);
tr.add(t,param.get(i,3));
tr.pi();
for (int p =0;p<n;p++){
for(int q=0;q<n;q++){
dtmp = exp(-10.0*param.get(i,0)*pow(fr.get(p,q)/n/param.get(i,1)-1,2)-2*param.get(i,2)*PI*pow(tr.get(p,q),2));
G[p][q][i] = dtmp;
}
}
//G(:,:,i)=exp(-10*param(i,1)*(fr/n/param(i,2)-1).^2-2*param(i,3)*pi*tr.^2);
}
/* for showing the figure
if nargout == 0
figure
for i=1:Nfilters
max(max(G(:,:,i)))
contour(fftshift(G(:,:,i)),[1 .7 .6],'r');
hold on
drawnow
end
axis('on')
axis('square')
axis('ij')
end*/
}
