void dump_grid_range_highlight(const Grid & grid,
ssize_t ix0, ssize_t iy0,
ssize_t ix1, ssize_t iy1,
ssize_t ixhigh, ssize_t iyhigh,
FILE * stream)
{
PVDEBUG("%zu %zu %zu %zu %zu %zu\n",
ix0, iy0, ix1, iy1, ixhigh, iyhigh);
fprintf(stream, " ");
for(ssize_t ix(ix0); ix <= ix1; ++ix)
if(ix == ixhigh) fprintf(stream, " ***********");
else fprintf(stream, " ");
fprintf(stream, " \n");
ssize_t iy(iy1);
for(/**/; iy != iy0; --iy){
const char * high(iy == iyhigh ? "*" : " ");
linesep(grid, stream, ix0, ix1, 0, " ");
line1(grid, stream, iy, ix0, ix1, 0, high);
line2(grid, stream, iy, ix0, ix1, 0, high);
line3(grid, stream, iy, ix0, ix1, 0, high);
line4(grid, stream, iy, ix0, ix1, 0, high);
}
const char * high(iy == iyhigh ? "*" : " ");
linesep(grid, stream, ix0, ix1, 0, " ");
line1(grid, stream, iy, ix0, ix1, 0, high);
line2(grid, stream, iy, ix0, ix1, 0, high);
line3(grid, stream, iy, ix0, ix1, 0, high);
line4(grid, stream, iy, ix0, ix1, 0, high);
linesep(grid, stream, ix0, ix1, 0, " ");
fprintf(stream, " ");
for(ssize_t ix(ix0); ix <= ix1; ++ix)
if(ix == ixhigh) fprintf(stream, " ***********");
else fprintf(stream, " ");
fprintf(stream, " \n");
}
void Mapper2d::
InitAddmask()
{
m_addmask_x0 = 0;
m_addmask_y0 = 0;
m_addmask_x1 = 0;
m_addmask_y1 = 0;
const ssize_t offset(static_cast<ssize_t>(ceil(grown_safe_distance / gridframe.Delta())));
for (ssize_t ix(-offset); ix <= offset; ++ix) {
const double x2(sqr(ix * gridframe.Delta()));
for (ssize_t iy(-offset); iy <= offset; ++iy) {
int const cost(ComputeCost(sqrt(sqr(iy * gridframe.Delta()) + x2)));
if (cost > m_travmap->freespace) {
m_addmask.insert(make_pair(index_t(ix, iy), cost));
// update the addmask's bounding box
if (ix < m_addmask_x0)
m_addmask_x0 = ix;
if (ix > m_addmask_x1)
m_addmask_x1 = ix;
if (iy < m_addmask_y0)
m_addmask_y0 = iy;
if (iy > m_addmask_y1)
m_addmask_y1 = iy;
}
}
}
}
void Mapper2d::
ClearAllObstacles(draw_callback * cb)
{
for (ssize_t ix(m_travmap->GetXBegin()); ix < m_travmap->GetXEnd(); ++ix)
for (ssize_t iy(m_travmap->GetYBegin()); iy < m_travmap->GetYEnd(); ++iy)
if ( ! m_travmap->IsFree(ix, iy))
m_travmap->SetFree(ix, iy, cb);
}
array(index_t xsize, index_t ysize, const value_t & init)
: data(new inner_t[xsize])
{
for (index_t ix(0); ix < xsize; ++ix) {
data[ix].reset(new value_t[ysize]);
for(index_t iy(0); iy < ysize; ++iy)
data[ix][iy] = init;
}
}
void dump_grid_range(const Grid & grid,
ssize_t ix0, ssize_t iy0, ssize_t ix1, ssize_t iy1,
FILE * stream)
{
PVDEBUG("%zu %zu %zu %zu\n", ix0, iy0, ix1, iy1);
const char * even("");
const char * oddsep;
const char * oddpre;
if (grid.GetNeighborhood() == Grid::SIX) {
oddsep = "+-----";
oddpre = " ";
}
else {
oddsep = even;
oddpre = even;
}
ssize_t iy(iy1);
const char * prefix;
for(/**/; iy != iy0; --iy){
if(iy % 2){
linesep(grid, stream, ix0, ix1, oddsep, 0);
prefix = oddpre;
}
else{
linesep(grid, stream, ix0, ix1, even, 0);
prefix = even;
}
line1(grid, stream, iy, ix0, ix1, prefix, 0);
line2(grid, stream, iy, ix0, ix1, prefix, 0);
line3(grid, stream, iy, ix0, ix1, prefix, 0);
line4(grid, stream, iy, ix0, ix1, prefix, 0);
}
if(iy % 2){
linesep(grid, stream, ix0, ix1, oddsep, 0);
prefix = oddpre;
}
else{
linesep(grid, stream, ix0, ix1, even, 0);
prefix = even;
}
line1(grid, stream, iy, ix0, ix1, prefix, 0);
line2(grid, stream, iy, ix0, ix1, prefix, 0);
line3(grid, stream, iy, ix0, ix1, prefix, 0);
line4(grid, stream, iy, ix0, ix1, prefix, 0);
if(iy % 2)
linesep(grid, stream, ix0, ix1, even, 0);
else
linesep(grid, stream, ix0, ix1, oddsep, 0);
}
Vector4f ColorGradient::GetColorWOErrorChecking(float f) const
{
f = Mathf::Clamp(f, OrderedNodes[0].Position, OrderedNodes[OrderedNodes.size() - 1].Position);
//Edge cases. Choose a random node side if the given value sits right on a Node.
if (f == OrderedNodes[0].Position)
{
return ((rand() % 9) < 5) ? OrderedNodes[0].LeftColor : OrderedNodes[0].RightColor;
}
if (f == OrderedNodes[OrderedNodes.size() - 1].Position)
{
return ((rand() % 9) < 5) ? OrderedNodes[OrderedNodes.size() - 1].LeftColor : OrderedNodes[OrderedNodes.size() - 1].RightColor;
}
//Count to the first node above the given position.
int index;
for (index = OrderedNodes.size() - 1; OrderedNodes[index].Position > f && index > 0; --index) { }
//Lerp between the two color values.
Vector4f leftCol = OrderedNodes[index].RightColor;
Vector4f rightCol = OrderedNodes[index + 1].LeftColor;
Interval posInt(OrderedNodes[index].Position, OrderedNodes[index + 1].Position, 0.001f, true, true);
Vector4b needReversed((unsigned char)(leftCol.x > rightCol.x), leftCol.y > rightCol.y, leftCol.z > rightCol.z, leftCol.w > rightCol.w);
float ref = posInt.Reflect(f);
float rx = (needReversed.x) ? ref : f,
ry = (needReversed.y) ? ref : f,
rz = (needReversed.z) ? ref : f,
rw = (needReversed.w) ? ref : f;
Interval ix(leftCol.x, rightCol.x, 0.001f, true, true),
iy(leftCol.y, rightCol.y, 0.001f, true, true),
iz(leftCol.z, rightCol.z, 0.001f, true, true),
iw(leftCol.w, rightCol.w, 0.001f, true, true);
rx = posInt.MapValue(ix, rx);
ry = posInt.MapValue(iy, ry);
rz = posInt.MapValue(iz, rz);
rw = posInt.MapValue(iw, rw);
Vector4f ret(rx, ry, rz, rw);
return ret;
}
/** \todo this implementation is a bit brute force... */
void NF1::
SetLocalDisk(GridFrame const & gframe,
grid_t & grid, position_t center,
double radius, double value)
{
index_t index(gframe.LocalIndex(center));
if(grid.ValidIndex(index))
grid[index] = value;
const index_t
minidx(gframe.LocalIndex(center.v0 - radius, center.v1 - radius));
const index_t
maxidx(gframe.LocalIndex(center.v0 + radius, center.v1 + radius));
const double radius2(sqr(radius));
for(ssize_t ix(minidx.v0); ix < maxidx.v0; ++ix)
for(ssize_t iy(minidx.v1); iy < maxidx.v1; ++iy)
if(grid.ValidIndex(ix, iy)){
const position_t point(gframe.LocalPoint(ix, iy) - center);
const double r2(sqr(point.v0) + sqr(point.v1));
if(r2 <= radius2)
grid[ix][iy] = value;
}
}
void dump_probabilities(const array<double> &prob,
size_t x0, size_t y0, size_t x1, size_t y1,
FILE * stream)
{
for(size_t iy(y1); iy >= y0; --iy){
for(size_t ix(x0); ix <= x1; ++ix)
fprintf(stream, "+-----");
fprintf(stream, "+\n");
for(size_t ix(x0); ix <= x1; ++ix){
const double value(prob[ix][iy]);
if(value < 0) fprintf(stream, "| < 0 ");
else if(value > 1) fprintf(stream, "| > 1 ");
else if(value < 1e-9) fprintf(stream, "|tiny ");
else if(value > 1-1e-9) fprintf(stream, "| one ");
else fprintf(stream, "|%5.2f", value);
}
fprintf(stream, "|\n");
}
for(size_t ix(x0); ix <= x1; ++ix)
fprintf(stream, "+-----");
fprintf(stream, "+\n");
}
void SBPLLatticePlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
if(!initialized_){
ros::NodeHandle private_nh("~/"+name);
ros::NodeHandle nh(name);
ROS_INFO("Name is %s", name.c_str());
private_nh.param("planner_type", planner_type_, string("ARAPlanner"));
private_nh.param("allocated_time", allocated_time_, 10.0);
private_nh.param("initial_epsilon",initial_epsilon_,3.0);
private_nh.param("environment_type", environment_type_, string("XYThetaLattice"));
private_nh.param("forward_search", forward_search_, bool(false));
private_nh.param("primitive_filename",primitive_filename_,string(""));
private_nh.param("force_scratch_limit",force_scratch_limit_,500);
double nominalvel_mpersecs, timetoturn45degsinplace_secs;
private_nh.param("nominalvel_mpersecs", nominalvel_mpersecs, 0.4);
private_nh.param("timetoturn45degsinplace_secs", timetoturn45degsinplace_secs, 0.6);
int lethal_obstacle;
private_nh.param("lethal_obstacle",lethal_obstacle,20);
lethal_obstacle_ = (unsigned char) lethal_obstacle;
inscribed_inflated_obstacle_ = lethal_obstacle_-1;
sbpl_cost_multiplier_ = (unsigned char) (costmap_2d::INSCRIBED_INFLATED_OBSTACLE/inscribed_inflated_obstacle_ + 1);
ROS_DEBUG("SBPL: lethal: %uz, inscribed inflated: %uz, multiplier: %uz",lethal_obstacle,inscribed_inflated_obstacle_,sbpl_cost_multiplier_);
costmap_ros_ = costmap_ros;
std::vector<geometry_msgs::Point> footprint = costmap_ros_->getRobotFootprint();
if ("XYThetaLattice" == environment_type_){
ROS_DEBUG("Using a 3D costmap for theta lattice\n");
env_ = new EnvironmentNAVXYTHETALAT();
}
else{
ROS_ERROR("XYThetaLattice is currently the only supported environment!\n");
exit(1);
}
// check if the costmap has an inflation layer
// Warning: footprint updates after initialization are not supported here
unsigned char cost_possibly_circumscribed_tresh = 0;
for(std::vector<boost::shared_ptr<costmap_2d::Layer> >::const_iterator layer = costmap_ros_->getLayeredCostmap()->getPlugins()->begin();
layer != costmap_ros_->getLayeredCostmap()->getPlugins()->end();
++layer) {
boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
if (!inflation_layer) continue;
cost_possibly_circumscribed_tresh = inflation_layer->computeCost(costmap_ros_->getLayeredCostmap()->getCircumscribedRadius());
}
if(!env_->SetEnvParameter("cost_inscribed_thresh",costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))){
ROS_ERROR("Failed to set cost_inscribed_thresh parameter");
exit(1);
}
if(!env_->SetEnvParameter("cost_possibly_circumscribed_thresh", costMapCostToSBPLCost(cost_possibly_circumscribed_tresh))){
ROS_ERROR("Failed to set cost_possibly_circumscribed_thresh parameter");
exit(1);
}
int obst_cost_thresh = costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE);
vector<sbpl_2Dpt_t> perimeterptsV;
perimeterptsV.reserve(footprint.size());
for (size_t ii(0); ii < footprint.size(); ++ii) {
sbpl_2Dpt_t pt;
pt.x = footprint[ii].x;
pt.y = footprint[ii].y;
perimeterptsV.push_back(pt);
}
bool ret;
try{
ret = env_->InitializeEnv(costmap_ros_->getCostmap()->getSizeInCellsX(), // width
costmap_ros_->getCostmap()->getSizeInCellsY(), // height
0, // mapdata
0, 0, 0, // start (x, y, theta, t)
0, 0, 0, // goal (x, y, theta)
0, 0, 0, //goal tolerance
perimeterptsV, costmap_ros_->getCostmap()->getResolution(), nominalvel_mpersecs,
timetoturn45degsinplace_secs, obst_cost_thresh,
primitive_filename_.c_str());
}
catch(SBPL_Exception e){
ROS_ERROR("SBPL encountered a fatal exception!");
ret = false;
}
if(!ret){
ROS_ERROR("SBPL initialization failed!");
exit(1);
}
for (ssize_t ix(0); ix < costmap_ros_->getCostmap()->getSizeInCellsX(); ++ix)
for (ssize_t iy(0); iy < costmap_ros_->getCostmap()->getSizeInCellsY(); ++iy)
env_->UpdateCost(ix, iy, costMapCostToSBPLCost(costmap_ros_->getCostmap()->getCost(ix,iy)));
if ("ARAPlanner" == planner_type_){
ROS_INFO("Planning with ARA*");
planner_ = new ARAPlanner(env_, forward_search_);
}
else if ("ADPlanner" == planner_type_){
ROS_INFO("Planning with AD*");
planner_ = new ADPlanner(env_, forward_search_);
}
else{
ROS_ERROR("ARAPlanner and ADPlanner are currently the only supported planners!\n");
exit(1);
}
ROS_INFO("[sbpl_lattice_planner] Initialized successfully");
plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
stats_publisher_ = private_nh.advertise<sbpl_lattice_planner::SBPLLatticePlannerStats>("sbpl_lattice_planner_stats", 1);
initialized_ = true;
}
}
void initialize_new_objects(parameter_t const& P, directory_structure_t const& ds,
geometric_info_t const& gi, object_info_t& oi,
vector<std::vector<std::string> > const &seq, int tt,
vector<CImg<unsigned char> > const& images,
vector<matrix<float> > const& grd,
vector<matrix<float> >& detected_rects)
{
int Ncam = seq.size();
vector<object_trj_t> & trlet_list=oi.trlet_list;
int nobj = trlet_list.size();
int num_new_obj = detected_rects(0).size1();
int T = seq[0].size();
int np = oi.model.size();
int num_scales = P.scales.size();
for(int oo=0; oo<num_new_obj; ++oo)
{
int nn = oi.curr_num_obj + oo;
trlet_list(nn).startt = tt;
trlet_list(nn).endt = tt;
trlet_list(nn).state = 1;
trlet_list(nn).trj = vector<matrix<float> >(Ncam);
for(int cam=0; cam<Ncam; ++cam)
{
trlet_list(nn).trj(cam) = scalar_matrix<float>(T, 4, 0);
}
trlet_list(nn).trj_3d = scalar_matrix<float>(T, 2, 0);
trlet_list(nn).hist_p = vector<matrix<float> >(Ncam);
trlet_list(nn).hist_q = vector<matrix<float> >(Ncam);
trlet_list(nn).fscores = vector<matrix<float> >(Ncam);
trlet_list(nn).scores = scalar_matrix<float>(Ncam, T, 0);
vector<candidate_array<Float> > cand_array(Ncam);
for(int cam=0; cam<Ncam; ++cam)
{
trlet_list(nn).fscores(cam) = scalar_matrix<float>(np*2, T, 0);
float w = detected_rects(cam)(oo, 2)-detected_rects(cam)(oo, 0);
float h = detected_rects(cam)(oo, 3)-detected_rects(cam)(oo, 1);
row(trlet_list(nn).trj(cam), tt) = row(detected_rects(cam), oo);
matrix<float> rects;
compute_part_rects(detected_rects(cam)(oo, 0), detected_rects(cam)(oo, 1),
w, h, oi.model, rects);
pmodel_t pmodel;
vector<float> br(row(detected_rects(cam), oo));
rects_to_pmodel_geom(br, gi.horiz_mean, pmodel);
oi.pmodel_list(cam, nn) = pmodel;
//collect_sift(grd(cam), );
matrix<float> hist_p, hist_q;
collect_hist(images(cam), rects, hist_p, hist_q);
trlet_list(nn).hist_p(cam) = hist_p;
trlet_list(nn).hist_q(cam) = hist_q;
std::vector<float> sxr, syr;
for(float v=-P.xrange/2; v<=P.xrange/2; v+=P.xstep)
{
sxr.push_back(v);
}
for(float v=-P.yrange/2; v<=P.yrange/2; v+=P.ystep)
{
syr.push_back(v);
}
vector<float> xr(sxr.size()), yr(syr.size());
std::copy(sxr.begin(), sxr.end(), xr.begin());
std::copy(syr.begin(), syr.end(), yr.begin());
float feetx = (trlet_list(nn).trj(cam)(tt, 0)
+trlet_list(nn).trj(cam)(tt, 2))/2;
float feety = trlet_list(nn).trj(cam)(tt, 3);
matrix<Float> cand_rects;
vector<Float> cand_scale;
matrix<int> cand_ijs;
enumerate_rects_inpoly(images(cam), oi.pmodel_list(cam, nn),
feetx, feety,
xr, yr, P.scales, gi.horiz_mean, gi.horiz_sig,
gi.polys_im(cam),
cand_rects, cand_scale,
cand_ijs, cand_array(cam));
vector<Float> cand_hist_score;
matrix<Float> hist_fscores;
real_timer_t timer;
get_cand_hist_score(images(cam), oi.model, P.logp1, P.logp2,
trlet_list(nn).hist_p(cam),
trlet_list(nn).hist_q(cam),
cand_rects,
cand_hist_score, hist_fscores);
vector<Float> cand_score=cand_hist_score;
std::cout<<"\t\t"<<cand_rects.size1()<<" rects, \tget_cand_hist_score time:"
<<timer.elapsed()/1000.0f<<"s."<<std::endl;
int idx_max = std::max_element(cand_score.begin(), cand_score.end())
- cand_score.begin();
column(trlet_list(nn).fscores(cam), tt) = row(hist_fscores, idx_max);
trlet_list(nn).scores(cam, tt) = cand_score(idx_max);
cand_array(cam).fill_score(cand_score, cand_ijs);
}//end for cam
real_timer_t timer;
ground_scoremap_t<Float> grd_scoremap;
combine_ground_score(cand_array, grd_scoremap, gi);
int best_y, best_x, best_s;
grd_scoremap.peak(best_y, best_x, best_s);
for(int cam=0; cam<Ncam; ++cam)
{
vector<double> bx(1), by(1), ix, iy;
bx <<= best_x; by <<= best_y;
apply_homography(gi.grd2img(cam), bx, by, ix, iy);
float hpre = oi.pmodel_list(cam, nn).hpre;
float cur_fy = iy(0);
float cur_fx = ix(0);
float cur_hy = gi.horiz_mean+hpre*(cur_fy-gi.horiz_mean);
float ds = P.scales(best_s)*(cur_fy-cur_hy)/oi.pmodel_list(cam, nn).bh;
float ww = ds*oi.pmodel_list(cam, nn).bw;
float hh = cur_fy - cur_hy;
vector<Float> tmp(4);
tmp <<= (cur_fx-ww/2), cur_hy, (cur_fx+ww/2), cur_fy;
row(trlet_list(nn).trj(cam), tt) = tmp;
//std::cout<<"trlet_list(nn).trj(cam)(tt, :)="
// <<row(trlet_list(nn).trj(cam), tt)<<std::endl;
}//endfor cam
}//endfor oo
oi.curr_num_obj += num_new_obj;
}
void initialize_new_objects(mpi::communicator& world,
parameter_t const& P, directory_structure_t const& ds,
geometric_info_t const& gi, object_info_t& oi,
vector<std::vector<std::string> > const &seq, int tt,
vector<CImg<unsigned char> > const& images,
vector<matrix<float> > const& grd,
vector<matrix<float> >& detected_rects)
{
int Ncam = seq.size();
vector<object_trj_t> & trlet_list=oi.trlet_list;
int nobj = trlet_list.size();
int num_new_obj = detected_rects(0).size1();
int T = seq[0].size();
int np = oi.model.size();
int num_scales = P.scales.size();
//std::cout<<"detected_rects="<<detected_rects<<std::endl;
for(int oo=0; oo<num_new_obj; ++oo)
{
int nn = oi.curr_num_obj + oo;
trlet_list(nn).startt = tt;
trlet_list(nn).endt = tt;
trlet_list(nn).state = 1;
trlet_list(nn).trj = vector<matrix<float> >(Ncam);
for(int cam=0; cam<Ncam; ++cam)
{
trlet_list(nn).trj(cam) = scalar_matrix<float>(T, 4, 0);
}
trlet_list(nn).trj_3d = scalar_matrix<float>(T, 2, 0);
trlet_list(nn).hist_p = vector<matrix<float> >(Ncam);
trlet_list(nn).hist_q = vector<matrix<float> >(Ncam);
trlet_list(nn).fscores = vector<matrix<float> >(Ncam);
trlet_list(nn).scores = scalar_matrix<float>(Ncam, T, 0);
vector<candidate_array<Float> > cand_array(Ncam);
for(int cam=0; cam<Ncam; ++cam)
{
trlet_list(nn).fscores(cam) = scalar_matrix<float>(np*2, T, 0);
float w = detected_rects(cam)(oo, 2)-detected_rects(cam)(oo, 0);
float h = detected_rects(cam)(oo, 3)-detected_rects(cam)(oo, 1);
row(trlet_list(nn).trj(cam), tt) = row(detected_rects(cam), oo);
matrix<float> rects;
compute_part_rects(detected_rects(cam)(oo, 0), detected_rects(cam)(oo, 1),
w, h, oi.model, rects);
pmodel_t pmodel;
vector<float> br(row(detected_rects(cam), oo));
rects_to_pmodel_geom(br, gi.horiz_mean, pmodel);
oi.pmodel_list(cam, nn) = pmodel;
//collect_sift(grd(cam), );
matrix<float> hist_p, hist_q;
collect_hist(images(cam), rects, hist_p, hist_q);
trlet_list(nn).hist_p(cam) = hist_p;
trlet_list(nn).hist_q(cam) = hist_q;
matrix<Float> cand_rects;
vector<Float> cand_scale;
matrix<int> cand_ijs;
if(0==world.rank())
{
std::vector<float> sxr, syr;
for(float v=-P.xrange/2; v<=P.xrange/2; v+=P.xstep)
{
sxr.push_back(v);
}
for(float v=-P.yrange/2; v<=P.yrange/2; v+=P.ystep)
{
syr.push_back(v);
}
vector<float> xr(sxr.size()), yr(syr.size());
std::copy(sxr.begin(), sxr.end(), xr.begin());
std::copy(syr.begin(), syr.end(), yr.begin());
float feetx = (trlet_list(nn).trj(cam)(tt, 0)
+trlet_list(nn).trj(cam)(tt, 2))/2;
float feety = trlet_list(nn).trj(cam)(tt, 3);
enumerate_rects_inpoly(images(cam), oi.pmodel_list(cam, nn),
feetx, feety,
xr, yr, P.scales, gi.horiz_mean, gi.horiz_sig,
gi.polys_im(tt, cam),
cand_rects, cand_scale,
cand_ijs, cand_array(cam));
}
mpi::broadcast(world, cand_rects, 0);
real_timer_t timer;
vector<Float> cand_hist_score(cand_rects.size1());
matrix<Float> hist_fscores;
range rrank(world.rank()*cand_rects.size1()/world.size(),
(world.rank()+1)*cand_rects.size1()/world.size());
matrix<Float> cand_rects_rank(project(cand_rects, rrank, range(0, 4)));
vector<Float> cand_hist_score_rank;
matrix<Float> hist_fscores_rank;
get_cand_hist_score(images(cam), oi.model, P.logp1, P.logp2,
trlet_list(nn).hist_p(cam),
trlet_list(nn).hist_q(cam),
cand_rects_rank,
cand_hist_score_rank, hist_fscores_rank);
if(world.rank()==0)
{
std::vector<vector<Float> > v1;
std::vector<matrix<Float> > v2;
mpi::gather(world, cand_hist_score_rank, v1, 0);
mpi::gather(world, hist_fscores_rank, v2, 0);
hist_fscores = matrix<Float>(cand_rects.size1(),
hist_fscores_rank.size2());
for(int r=0; r<world.size(); ++r)
{
int start = r*cand_rects.size1()/world.size();
for(int vv=0; vv<v1[r].size(); ++vv)
{
cand_hist_score(start+vv) = v1[r](vv);
}
for(int vv=0; vv<v2[r].size1(); ++vv)
{
row(hist_fscores, start+vv) = row(v2[r], vv);
}
}
}
else
{
mpi::gather(world, cand_hist_score_rank, 0);
mpi::gather(world, hist_fscores_rank, 0);
}
mpi::broadcast(world, cand_hist_score, 0);
mpi::broadcast(world, hist_fscores, 0);
vector<Float> cand_score=cand_hist_score;
if(0==world.rank())
std::cout<<"\t\t"<<cand_rects.size1()<<" rects, \tget_cand_hist_score time:"
<<timer.elapsed()/1000.0f<<"s."<<std::endl;
if(0==world.rank())
{
int idx_max = std::max_element(cand_score.begin(), cand_score.end())
- cand_score.begin();
column(trlet_list(nn).fscores(cam), tt) = row(hist_fscores, idx_max);
trlet_list(nn).scores(cam, tt) = cand_score(idx_max);
cand_array(cam).fill_score(cand_score, cand_ijs);
}
mpi::broadcast(world, cand_array(cam), 0);
mpi::broadcast(world, trlet_list(nn).scores(cam, tt), 0);
vector<Float> fscore_col;
if(0==world.rank())
{
fscore_col = column(trlet_list(nn).fscores(cam), tt);
}
mpi::broadcast(world, fscore_col, 0);
if(0!=world.rank())
{
column(trlet_list(nn).fscores(cam), tt) = fscore_col;
}
}//end for cam
int best_y, best_x, best_s;
if(0==world.rank())
{
ground_scoremap_t<Float> grd_scoremap;
combine_ground_score(tt, cand_array, grd_scoremap, gi);
grd_scoremap.peak(best_y, best_x, best_s);
}
mpi::broadcast(world, best_y, 0);
mpi::broadcast(world, best_x, 0);
trlet_list(nn).trj_3d(tt, 0) = best_x;
trlet_list(nn).trj_3d(tt, 1) = best_y;
for(int cam=0; cam<Ncam; ++cam)
{
vector<Float> trj_row(4);
if(0==world.rank())
{
vector<double> bx(1), by(1), ix, iy;
bx <<= best_x; by <<= best_y;
apply_homography(gi.grd2img(tt, cam), bx, by, ix, iy);
float hpre = oi.pmodel_list(cam, nn).hpre;
float cur_fy = iy(0);
float cur_fx = ix(0);
float cur_hy = gi.horiz_mean+hpre*(cur_fy-gi.horiz_mean);
float ds = P.scales(best_s)*(cur_fy-cur_hy)/oi.pmodel_list(cam, nn).bh;
float ww = ds*oi.pmodel_list(cam, nn).bw;
float hh = cur_fy - cur_hy;
trj_row <<= (cur_fx-ww/2), cur_hy, (cur_fx+ww/2), cur_fy;
}
mpi::broadcast(world, trj_row, 0);
row(trlet_list(nn).trj(cam), tt) = trj_row;
}//endfor cam
}//endfor oo
oi.curr_num_obj += num_new_obj;
}
void
Elastic::FAC::pack_strain(double* buffer,
const SAMRAI::hier::Patch& patch,
const SAMRAI::hier::Box& region,
const int &depth) const
{
boost::shared_ptr<SAMRAI::pdat::SideData<double> > v_ptr=
boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> >
(patch.getPatchData(v_id));
SAMRAI::pdat::SideData<double>& v = *v_ptr;
boost::shared_ptr<SAMRAI::pdat::CellData<double> > dv_diagonal;
boost::shared_ptr<SAMRAI::pdat::SideData<double> > dv_mixed;
if(!faults.empty())
{
dv_diagonal=boost::dynamic_pointer_cast<SAMRAI::pdat::CellData<double> >
(patch.getPatchData(dv_diagonal_id));
dv_mixed=boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> >
(patch.getPatchData(dv_mixed_id));
}
const Gamra::Dir dim=d_dim.getValue();
if(have_embedded_boundary())
{
boost::shared_ptr<SAMRAI::pdat::SideData<double> > level_set_ptr;
if(have_embedded_boundary())
level_set_ptr=boost::dynamic_pointer_cast<SAMRAI::pdat::SideData<double> >
(patch.getPatchData(level_set_id));
}
Gamra::Dir ix(Gamra::Dir::from_int(depth/dim)),
iy(Gamra::Dir::from_int(depth%dim));
const SAMRAI::hier::Index zero(SAMRAI::hier::Index::getZeroIndex(d_dim));
SAMRAI::hier::Index ip(zero), jp(zero);
ip[ix]=1;
jp[iy]=1;
const double *dx(boost::dynamic_pointer_cast
<SAMRAI::geom::CartesianPatchGeometry>
(patch.getPatchGeometry())->getDx());
SAMRAI::pdat::CellIterator iend(SAMRAI::pdat::CellGeometry::end(region));
for(SAMRAI::pdat::CellIterator
icell(SAMRAI::pdat::CellGeometry::begin(region));
icell!=iend; ++icell)
{
const SAMRAI::pdat::SideIndex
s(*icell,ix,SAMRAI::pdat::SideIndex::Lower);
if(ix==iy)
{
double diff(v(s+ip)-v(s));
if(!faults.empty())
diff-=(*dv_diagonal)(*icell,ix);
*buffer=diff/dx[ix];
}
else
{
const int ix_iy(index_map(ix,iy,dim));
double diff(- v(s-jp) - v(s+ip-jp) + v(s+jp) + v(s+ip+jp));
if(!faults.empty())
diff+=(*dv_mixed)(s,ix_iy+1) - (*dv_mixed)(s-jp,ix_iy)
+ (*dv_mixed)(s+ip,ix_iy+1) - (*dv_mixed)(s+ip-jp,ix_iy)
+ (*dv_mixed)(s+jp,ix_iy+1) - (*dv_mixed)(s,ix_iy)
+ (*dv_mixed)(s+ip+jp,ix_iy+1) - (*dv_mixed)(s+ip,ix_iy);
*buffer=diff/(4*dx[iy]);
}
++buffer;
}
}
