// Enlarge to bound second rect
void
Rectangle::Unite(Rectangle* pRect2)
{
x1 = AMin(x1, pRect2->x1);
y1 = AMin(y1, pRect2->y1);
x2 = AMax(x2, pRect2->x2);
y2 = AMax(y2, pRect2->y2);
area = CalcArea();
}
// Enlarge to bound second rect
void
TClipRect::Unite(TClipRect* pRect2)
{
x1 = AMin(x1, pRect2->x1);
y1 = AMin(y1, pRect2->y1);
x2 = AMax(x2, pRect2->x2);
y2 = AMax(y2, pRect2->y2);
area = CalcArea();
}
void
Rectangle::Construct(Rectangle* r)
{
x1 = r->x1;
y1 = r->y1;
x2 = r->x2;
y2 = r->y2;
owner = r->owner;
area = CalcArea();
}
void
TClipRect::Construct(TClipRect* r)
{
x1 = r->x1;
y1 = r->y1;
x2 = r->x2;
y2 = r->y2;
owner = r->owner;
area = CalcArea();
}
void
TClipRect::Construct(Rectangle* r)
{
x1 = r->x1;
y1 = r->y1;
x2 = r->x2 + 1;
y2 = r->y2 + 1;
owner = r->owner;
area = CalcArea();
}
void amclPoseArrayWeightedCallback(const amcl::PoseArrayWeighted::ConstPtr &msg) {
pose_array_weighted_.clear();
geometry_msgs::PoseStamped in;
in.header = msg->header;
for (int i = 0; i < (int) msg->poses.size(); i++) {
in.pose = msg->poses[i];
geometry_msgs::PoseStamped result;
try {
tf_->waitForTransform(global_frame_, base_frame_, msg->header.stamp, ros::Duration(0.3));
tf_->transformPose(base_frame_, in, result);
pose_array_weighted_.push_back(std::make_pair(msg->weights[i], result));
}
catch (tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
ROS_ERROR("point transform failed");
};
}
// if (!convex_ || orca_) {
std::vector <double> circle, convex;
for (eps_ = 0; eps_ <= 1; eps_ += 0.05) {
computeNewLocUncertainty();
computeNewMinkowskiFootprint();
double area = CalcArea(minkowski_footprint_);
//ROS_ERROR("eps = %f", eps_);
//ROS_ERROR("radius = %f, area = %f", cur_loc_unc_radius_, M_PI*sqr(cur_loc_unc_radius_));
circle.push_back(M_PI * sqr(cur_loc_unc_radius_));
convex.push_back(area);
//std::sort(minkowski_footprint_.begin(),minkowski_footprint_.end(), comparePoint);
//ROS_ERROR("area conv_ = %f", area);
//ROS_ERROR("furthest point= %f", collvoid::abs(minkowski_footprint_[int(minkowski_footprint_.size())-1]));
}
for (int i = 0; i < (int) circle.size(); i++) {
std::cout << circle[i] << ", ";
}
for (int i = 0; i < (int) circle.size(); i++) {
std::cout << convex[i] << ", ";
}
std::cout << ";" << std::endl;
}
/* Adds cell to list.
* Returns 1 in case of an error, 0 otherwise.
*/
static int AddCell(
DATA **list, /* read-write list of cells */
RASTER *raster, /* read-write raster to modify */
size_t *nrList, /* read-write number of items in list */
const POINT2D *inputCell, /* polygon of input cell */
const POINT2D *outputCell,/* polygon of output cell */
const REAL8 *currRow, /* current row */
const MAP *in, /* input map */
size_t nrMaps, /* nr. of input maps */
size_t c, /* current column number */
size_t nrCoverCells, /* nr. of non-MV cells for coverage */
BOOL aligned, /* maps are aligned */
REAL8 angle) /* angle of output map */
{
REAL8 value;
(*nrList)++;
if((*list = (DATA *)ChkRealloc(*list, *nrList * sizeof(DATA)))
== NULL)
return 1;
value = currRow[c];
if(!IsMV(in, &value))
{
double area; /* area of overlap */
area = CalcArea(inputCell, outputCell, aligned);
if(area != 0 && nrCoverCells > 0 && nrMaps > 1)
ModRaster(raster, outputCell, inputCell, angle);
(*list)[*nrList - 1].area = area;
(*list)[*nrList - 1].value = value;
}
else
{ /* no valid value */
(*list)[*nrList - 1].area = 0;
(*list)[*nrList - 1].value = 0;
}
return 0;
}
float Poly::bs_getArea() const {
return CalcArea(point[0], point[1], point[2]);
}
int main(int argc, char** argv) {
int i;
double t_opt = 0.;
param_ param, param_0;
gsl_complex **K, *W, *CW, *dW, *complex_rot;
arr_info_ arr_info;
double *argW, *absW;
double *rec, *prec;
pr_max_ *pr_max;
double AREA, AREA_0=1000.;
lattice_point_ **nghb;
if(argc!=3){
printf("Usage: recall/precision threshold, random seed\n");
exit(1);
}
/*
* Param initialization
*/
param.idum = (long *)malloc(sizeof(long)); // pointer to long integer for random number generator
*param.idum = atoi(argv[2]); // initialization to random number
param.thresh = HeaviSide; // function pointer for threshold
param.T = 2.0;
param.T_opt = 1.0;
param.dt = 0.01;
param.L = 100;
param.L_sub = 5;
param.int_range_rat = 3;
param.sigma = 0.008;
param.mu = 15;
param.Kinh = 0.012;
param.Kloc = 1.;
param.d = 0.25;
param.test_d = 4;
param.excl_d = 8;
param.M = 1.;
param.delta = 5.;
param.A = 5.;
param.N_max = 2;
param.kappa_upp = 0.6;
param.kappa_low = 0.4;
param.ecc_upp = 0.6;
param.ecc_low = 0.4;
param.kappa_xy = 0.1;
param.mask_min = 2;
param.mask_max = 4;
param.kappa_mask = 0.25;
param.pr_range = atoi(argv[1]);
param.clutter_thresh = 0.15;
param.excl_thresh = 1/2;
param.temp = 0.005;
param.mut = 0.01;
param.gnuplot = 1; // 0 for screen and 1 for print to file
/****
PIPE TO GNUPLOT
***/
FILE *pipe=popen("gnuplot -persist","w");
fprintf(pipe,"set size square\n");
fprintf(pipe,"unset key\n");
if(param.gnuplot == 1){
fprintf(pipe,"set term post enhanced color \"Helvetica\" 20\n");
}
fflush(pipe);
/*
* Specify the precision/recall range
* and initiate pr_max.
*/
rec =(double *)malloc(param.pr_range*sizeof(double));
prec =(double *)malloc(param.pr_range*sizeof(double));
param.pr_thresh =(double *)malloc(param.pr_range*sizeof(double));
for(i=0;i<param.pr_range;i++)
param.pr_thresh[i] = (double) i/param.pr_range;
pr_max = (pr_max_ *)malloc(param.pr_range*sizeof(pr_max_));
/*
* Need to define param_0.
*/
param_0 = param;
/*
* Kernel Initialization
*/
nghb = NghbInit(param, &arr_info, param.int_range_rat/sqrt(2*param.sigma));
K = KernelInit(param);
/*
* Initializing the rW, pW, eW size
* variable.
*/
W=(gsl_complex *)malloc(sizeof(gsl_complex)*param.L*param.L);
CW=(gsl_complex *)malloc(sizeof(gsl_complex)*param.L*param.L);
dW=(gsl_complex *)malloc(sizeof(gsl_complex)*param.L*param.L);
complex_rot=(gsl_complex *)malloc(sizeof(gsl_complex)*param.L*param.L);
argW=(double *)malloc(sizeof(double)*param.L*param.L);
absW=(double *)malloc(sizeof(double)*param.L*param.L);
/*
* Optimization loop
*/
while(t_opt < param.T_opt){
t_opt += OneSim(param, pipe, arr_info, nghb, K, W, CW, dW, complex_rot,
argW, absW, rec, prec, pr_max);
/*
* Do we keep or reject the current choice
* of parameters?
*/
AREA = CalcArea(param, pr_max);
if( 1./(1.+exp((AREA - AREA_0)/param.temp)) > ran1(param.idum) ){
param_0 = param;
AREA_0 = AREA;
}
param = NewParam(param_0);
/*
* Here we print out the current choice of
* parameters
*/
printf("%e %e %e %e %e %e %e\n", t_opt, param_0.sigma, param_0.mu,
param_0.Kinh, param_0.delta, param_0.A, AREA_0);
fflush(stdout);
} //End optimization loop;
free(param.idum);
free(W);
free(CW);
free(dW);
free(absW);
free(argW);
free(rec);
free(prec);
free(param.pr_thresh);
free(pr_max);
free2d((void **) nghb, param.L*param.L);
free2d((void **) K, 4*param.L+1);
pclose(pipe);
return (EXIT_SUCCESS);
}
void CGeoTri::UpdateArea()
{
m_fArea = CalcArea();
}
