/**
* Internal worker for RTSemXRoadsNSEnter and RTSemXRoadsEWEnter.
*
* @returns IPRT status code.
* @param pThis The semaphore instance.
* @param fDir The direction.
* @param uCountShift The shift count for getting the count.
* @param fCountMask The mask for getting the count.
* @param uWaitCountShift The shift count for getting the wait count.
* @param fWaitCountMask The mask for getting the wait count.
*/
DECL_FORCE_INLINE(int) rtSemXRoadsEnter(RTSEMXROADSINTERNAL *pThis, uint64_t fDir,
uint64_t uCountShift, uint64_t fCountMask,
uint64_t uWaitCountShift, uint64_t fWaitCountMask)
{
uint64_t u64OldState;
uint64_t u64State;
u64State = ASMAtomicReadU64(&pThis->u64State);
u64OldState = u64State;
add_hist(u64State, u64OldState, fDir, "enter");
for (;;)
{
if ((u64State & RTSEMXROADS_DIR_MASK) == (fDir << RTSEMXROADS_DIR_SHIFT))
{
/* It flows in the right direction, try follow it before it changes. */
uint64_t c = (u64State & fCountMask) >> uCountShift;
c++;
Assert(c < 8*_1K);
u64State &= ~fCountMask;
u64State |= c << uCountShift;
if (ASMAtomicCmpXchgU64(&pThis->u64State, u64State, u64OldState))
{
add_hist(u64State, u64OldState, fDir, "enter-simple");
break;
}
}
else if ((u64State & (RTSEMXROADS_CNT_NS_MASK | RTSEMXROADS_CNT_EW_MASK)) == 0)
int main(int np, char ** par)
{
int n;
char *process=NULL;
FILE*f;
if(np<3)
{ printf(" This routine is intended to sum the distributions produced\n"
" in calcHEP numerical sessions (files dist_#).\n"
" The names of files must be submitted as parameters\n"
" Resulting distribution is directed to stdout(screen)\n");
return 1;
}
for(n=1;n<np;n++)
{
f=fopen(par[n],"r");
if(!f) return 2;
if(add_hist(f,&process)) {fclose(f);return 3;}
fclose(f);
}
wrt_hist2(stdout,process);
return 0;
}
int merge_check(Puzzle *puz, Solution *sol)
{
MergeElem *m;
//dir_t z;
int found= 0;
for (m= merge_list; m != NULL; m= m->next)
{
if (m->maxc == merge_no && m->cell != NULL)
{
if (VM)
{
printf("M: FOUND MERGED CONSEQUENCE ON CELL (%d,%d) BITS ",
m->cell->line[0], m->cell->line[1]);
dump_bits(stdout, puz, m->bit);
printf("\n");
}
/* Add to history as a necessary consequence */
add_hist(puz, m->cell, 0);
/* Set the new value in the cell */
#ifdef LIMITCOLORS
oldval[0]= m->cell->bit[0];
m->cell->bit[0]&= ~m->bit[0];
#else
for (z= 0; z < fbit_size; z++)
{
oldval[z]= m->cell->bit[z];
m->cell->bit[z]&= ~m->bit[z];
}
#endif
if (puz->ncolor <= 2)
m->cell->n= 1;
else
count_cell(puz,m->cell);
if (m->cell->n == 1) solved_a_cell(puz, m->cell,1);
/* Add rows/columns containing this cell to the job list */
add_jobs(puz, sol, -1, m->cell, 0, oldval);
found= 1;
}
/* Reset to unused state */
m->cell= NULL;
}
if (VM && !found) printf("M: NO MERGE CONSEQUENCES\n");
merge_list= NULL;
merge_no= -1;
merging= 0;
return found;
}
void guess_cell(Puzzle *puz, Solution *sol, Cell *cell, color_t c)
{
dir_t k;
Hist *h;
/* Save old cell in backtrack history */
h= add_hist(puz, cell, 1);
/* Set just that one color */
cell->n= 1;
fbit_setonly(cell->bit,c);
solved_a_cell(puz,cell, 1);
/* Put all crossing lines onto the job list */
add_jobs(puz, sol, -1, cell, 0, h->bit);
}
void sell_handler(struct player_t *player)
{
output("Enter the ticker symbol of the stock you wish to sell: ");
char *sym = read_ticker();
output("Getting stock information...\n");
struct stock_t *stock = NULL;
stock = find_stock(player, sym);
if(!stock)
{
output("Couldn't find '%s' in portfolio.\n", sym);
free(sym);
return;
}
free(sym);
output("Updating prices...\n");
get_stock_info(stock->symbol, &stock->current_price, &stock->fullname);
output("You currently own %llu share(s) of '%s' (%s) valued at $%llu.%02llu each.\n",
stock->count, stock->fullname, stock->symbol, stock->current_price.cents / 100, stock->current_price.cents % 100);
output("How many shares do you wish to sell? ");
ullong sell_count = read_int();
if(!sell_count)
{
output("Sale cancelled.\n");
return;
}
if(stock->count < sell_count)
{
output("You don't own enough shares!\n");
return;
}
ullong sell_total = stock->current_price.cents * sell_count;
output("This will sell %llu share(s) for $%llu.%02llu total.\nProceed? ", sell_count, sell_total / 100, sell_total % 100);
char *response = read_string();
if(response[0] == 'y')
{
stock->count -= sell_count;
/* commented out to preserve history */
#if 0
if(stock->count == 0)
{
/* remove this item from the portfolio */
memmove(player->portfolio + stock_idx, player->portfolio + stock_idx + 1, sizeof(struct stock_t) * (player->portfolio_len - stock_idx - 1));
player->portfolio_len -= 1;
player->portfolio = realloc(player->portfolio, sizeof(struct stock_t) * player->portfolio_len);
}
#endif
player->cash.cents += sell_total;
add_hist(stock, SELL, sell_count);
output("%llu share(s) sold for $%llu.%02llu total.\n", sell_count, sell_total / 100, sell_total % 100);
}
else
{
output("Not confirmed.\n");
}
free(response);
}
int main (int argc, char** argv)
{
//ROS Initialization
ros::init(argc, argv, "detecting_people");
ros::NodeHandle nh;
ros::Rate rate(13);
ros::Subscriber state_sub = nh.subscribe("followme_state", 5, &stateCallback);
ros::Publisher people_pub = nh.advertise<frmsg::people>("followme_people", 5);
frmsg::people pub_people_;
CloudConverter* cc_ = new CloudConverter();
while (!cc_->ready_xyzrgb_)
{
ros::spinOnce();
rate.sleep();
if (!ros::ok())
{
printf("Terminated by Control-c.\n");
return -1;
}
}
// Input parameter from the .yaml
std::string package_path_ = ros::package::getPath("detecting_people") + "/";
cv::FileStorage* fs_ = new cv::FileStorage(package_path_ + "parameters.yml", cv::FileStorage::READ);
// Algorithm parameters:
std::string svm_filename = package_path_ + "trainedLinearSVMForPeopleDetectionWithHOG.yaml";
std::cout << svm_filename << std::endl;
float min_confidence = -1.5;
float min_height = 1.3;
float max_height = 2.3;
float voxel_size = 0.06;
Eigen::Matrix3f rgb_intrinsics_matrix;
rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics
// Read if some parameters are passed from command line:
pcl::console::parse_argument (argc, argv, "--svm", svm_filename);
pcl::console::parse_argument (argc, argv, "--conf", min_confidence);
pcl::console::parse_argument (argc, argv, "--min_h", min_height);
pcl::console::parse_argument (argc, argv, "--max_h", max_height);
// Read Kinect live stream:
PointCloudT::Ptr cloud_people (new PointCloudT);
cc_->ready_xyzrgb_ = false;
while ( !cc_->ready_xyzrgb_ )
{
ros::spinOnce();
rate.sleep();
}
pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud = cc_->msg_xyzrgb_;
// Display pointcloud:
pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
viewer.setCameraPosition(0,0,-2,0,-1,0,0);
// Add point picking callback to viewer:
struct callback_args cb_args;
PointCloudT::Ptr clicked_points_3d (new PointCloudT);
cb_args.clicked_points_3d = clicked_points_3d;
cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);
viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args);
std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;
// Spin until 'Q' is pressed:
viewer.spin();
std::cout << "done." << std::endl;
//cloud_mutex.unlock ();
// Ground plane estimation:
Eigen::VectorXf ground_coeffs;
ground_coeffs.resize(4);
std::vector<int> clicked_points_indices;
for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
clicked_points_indices.push_back(i);
pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs);
std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;
// Initialize new viewer:
pcl::visualization::PCLVisualizer viewer("PCL Viewer"); // viewer initialization
viewer.setCameraPosition(0,0,-2,0,-1,0,0);
// Create classifier for people detection:
pcl::people::PersonClassifier<pcl::RGB> person_classifier;
person_classifier.loadSVMFromFile(svm_filename); // load trained SVM
// People detection app initialization:
pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector; // people detection object
people_detector.setVoxelSize(voxel_size); // set the voxel size
people_detector.setIntrinsics(rgb_intrinsics_matrix); // set RGB camera intrinsic parameters
people_detector.setClassifier(person_classifier); // set person classifier
people_detector.setHeightLimits(min_height, max_height); // set person classifier
// people_detector.setSensorPortraitOrientation(true); // set sensor orientation to vertical
// For timing:
static unsigned count = 0;
static double last = pcl::getTime ();
int people_count = 0;
histogram* first_hist;
int max_people_num = (int)fs_->getFirstTopLevelNode()["max_people_num"];
// Main loop:
while (!viewer.wasStopped() && ros::ok() )
{
if ( current_state == 1 )
{
if ( cc_->ready_xyzrgb_ ) // if a new cloud is available
{
// std::cout << "In state 1!!!!!!!!!!" << std::endl;
std::vector<float> x;
std::vector<float> y;
std::vector<float> depth;
cloud = cc_->msg_xyzrgb_;
PointCloudT::Ptr cloud_new(new PointCloudT(*cloud));
cc_->ready_xyzrgb_ = false;
// Perform people detection on the new cloud:
std::vector<pcl::people::PersonCluster<PointT> > clusters; // vector containing persons clusters
people_detector.setInputCloud(cloud_new);
people_detector.setGround(ground_coeffs); // set floor coefficients
people_detector.compute(clusters); // perform people detection
ground_coeffs = people_detector.getGround(); // get updated floor coefficients
// Draw cloud and people bounding boxes in the viewer:
viewer.removeAllPointClouds();
viewer.removeAllShapes();
pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
unsigned int k = 0;
std::vector<pcl::people::PersonCluster<PointT> >::iterator it;
std::vector<pcl::people::PersonCluster<PointT> >::iterator it_min;
float min_z = 10.0;
float histo_dist_min = 2.0;
int id = -1;
for(it = clusters.begin(); it != clusters.end(); ++it)
{
if(it->getPersonConfidence() > min_confidence) // draw only people with confidence above a threshold
{
x.push_back((it->getTCenter())[0]);
y.push_back((it->getTCenter())[1]);
depth.push_back(it->getDistance());
// draw theoretical person bounding box in the PCL viewer:
/*pcl::copyPointCloud( *cloud, it->getIndices(), *cloud_people);
if ( people_count == 0 )
{
first_hist = calc_histogram_a( cloud_people );
people_count++;
it->drawTBoundingBox(viewer, k);
}
else if ( people_count <= 10 )
{
histogram* hist_tmp = calc_histogram_a( cloud_people );
add_hist( first_hist, hist_tmp );
it->drawTBoundingBox(viewer, k);
people_count++;
}*/
pcl::copyPointCloud( *cloud, it->getIndices(), *cloud_people);
if ( people_count < 11 )
{
if ( it->getDistance() < min_z )
{
it_min = it;
min_z = it->getDistance();
}
}
else if ( people_count == 11 )
{
normalize_histogram( first_hist );
people_count++;
histogram* hist_tmp = calc_histogram( cloud_people );
float tmp = histo_dist_sq( first_hist, hist_tmp );
std::cout << "The histogram distance is " << tmp << std::endl;
histo_dist_min = tmp;
it_min = it;
id = k;
}
else
{
histogram* hist_tmp = calc_histogram( cloud_people );
float tmp = histo_dist_sq( first_hist, hist_tmp );
std::cout << "The histogram distance is " << tmp << std::endl;
if ( tmp < histo_dist_min )
{
histo_dist_min = tmp;
it_min = it;
id = k;
}
}
k++;
//std::cout << "The data of the center is " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].x << " " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].y << " " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].z << std::endl;
//std::cout << "The size of the people cloud is " << cloud_people->points.size() << std::endl;
std::cout << "The " << k << " person's distance is " << it->getDistance() << std::endl;
}
}
pub_people_.x = x;
pub_people_.y = y;
pub_people_.depth = depth;
if ( k > 0 )
{
if ( people_count <= 11 )
{
pcl::copyPointCloud( *cloud, it_min->getIndices(), *cloud_people);
if ( people_count == 0)
{
first_hist = calc_histogram_a( cloud_people );
people_count++;
it_min->drawTBoundingBox(viewer, 1);
}
else if ( people_count < 11 )
{
histogram* hist_tmp = calc_histogram_a( cloud_people );
add_hist( first_hist, hist_tmp );
it_min->drawTBoundingBox(viewer, 1);
people_count++;
}
}
else
{
pub_people_.id = k-1;
if ( histo_dist_min < 1.3 )
{
it_min->drawTBoundingBox(viewer, 1);
std::cout << "The minimum distance of the histogram is " << histo_dist_min << std::endl;
std::cout << "The vector is " << it_min->getTCenter() << std::endl << "while the elements are " << (it_min->getTCenter())[0] << " " << (it_min->getTCenter())[1] << std::endl;
}
else
{
pub_people_.id = -1;
}
}
}
else
{
pub_people_.id = -1;
}
pub_people_.header.stamp = ros::Time::now();
people_pub.publish(pub_people_);
std::cout << k << " people found" << std::endl;
viewer.spinOnce();
ros::spinOnce();
// Display average framerate:
if (++count == 30)
{
double now = pcl::getTime ();
std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" << std::endl;
count = 0;
last = now;
}
//cloud_mutex.unlock ();
}
}
else
{
viewer.spinOnce();
ros::spinOnce();
// std::cout << "In state 0!!!!!!!!!" << std::endl;
}
}
return 0;
}
