void f()
{
#if defined BOOST_THREAD_USES_CHRONO
time_point t0 = Clock::now();
BOOST_TEST(!m.try_lock());
BOOST_TEST(!m.try_lock());
BOOST_TEST(!m.try_lock());
while (!m.try_lock())
;
time_point t1 = Clock::now();
m.unlock();
ns d = t1 - t0 - ms(250);
// This test is spurious as it depends on the time the thread system switches the threads
BOOST_TEST(d < ns(50000000)+ms(1000)); // within 50ms
#else
//time_point t0 = Clock::now();
//BOOST_TEST(!m.try_lock());
//BOOST_TEST(!m.try_lock());
//BOOST_TEST(!m.try_lock());
while (!m.try_lock())
;
//time_point t1 = Clock::now();
m.unlock();
//ns d = t1 - t0 - ms(250);
// This test is spurious as it depends on the time the thread system switches the threads
//BOOST_TEST(d < ns(50000000)+ms(1000)); // within 50ms
#endif
}
void f()
{
time_point t0 = Clock::now();
BOOST_TEST(!m.try_lock());
BOOST_TEST(!m.try_lock());
BOOST_TEST(!m.try_lock());
while (!m.try_lock())
;
time_point t1 = Clock::now();
m.unlock();
ns d = t1 - t0 - ms(250);
// This test is spurious as it depends on the time the thread system switches the threads
BOOST_TEST(d < ns(50000000)+ms(1000)); // within 50ms
}
/* ---[ */
int
main(int argc, char** argv)
{
if (argc > 1)
{
for (int i = 1; i < argc; i++)
{
if (std::string(argv[i]) == "-h")
{
printHelp(argc, argv);
return (-1);
}
}
}
std::string device_id = "";
pcl::console::parse_argument(argc, argv, "-dev", device_id);
cld.reset(new pcl::visualization::PCLVisualizer(argc, argv, "OpenNI Viewer"));
cld->setBackgroundColor(0, 0, 0);//背景を白色（255,255,255）に。
cld->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "OpenNICloud");
cld->addCoordinateSystem(1.0);
cld->initCameraParameters();
std::string mouseMsg3D("Mouse coordinates in PCL Visualizer");
std::string keyMsg3D("Key event for PCL Visualizer");
cld->registerMouseCallback(&mouse_callback, (void*)(&mouseMsg3D));
cld->registerKeyboardCallback(&keyboard_callback, (void*)(&keyMsg3D));
pcl::io::loadPCDFile("data\\robot\\raw_0.pcd", *g_cloud);
bool cld_init = false;
// Loop
while (!cld->wasStopped())
{
// Render and process events in the two interactors
cld->spinOnce();
// Add the cloud
if (g_cloud && cld_mutex.try_lock())
{
if (!cld_init)
{
cld->getRenderWindow()->SetSize(g_cloud->width, g_cloud->height);
cld->getRenderWindow()->SetPosition(g_cloud->width, 0);
cld_init = !cld_init;
}
if (!cld->updatePointCloud(g_cloud, "OpenNICloud"))
{
cld->addPointCloud(g_cloud, "OpenNICloud");
cld->resetCameraViewpoint("OpenNICloud");
}
cld_mutex.unlock();
}
boost::this_thread::sleep(boost::posix_time::microseconds(100));
}
}
void
cloud_cb (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr & cloud)
{
if (mutex_.try_lock ())
{
cloud_ = cloud;
mutex_.unlock ();
}
}
void
depth_image_cb (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image)
{
if (depth_image_mutex.try_lock ())
{
depth_image_ptr = depth_image;
depth_image_mutex.unlock ();
received_new_depth_data = true;
}
}
void
depth_image_cb (const openni_wrapper::DepthImage::Ptr& depth_image)
{
if (depth_image_mutex.try_lock ())
{
depth_image_ptr = depth_image;
depth_image_mutex.unlock ();
received_new_depth_data = true;
}
}
/* ---[ */
int
main (int argc, char** argv)
{
srand (time (0));
if (argc > 1)
{
for (int i = 1; i < argc; i++)
{
if (std::string (argv[i]) == "-h")
{
printHelp (argc, argv);
return (-1);
}
}
}
p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "OpenNI Viewer"));
std::string device_id = "";
pcl::console::parse_argument (argc, argv, "-dev", device_id);
pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
EventHelper h;
boost::function<void(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&) > f = boost::bind (&EventHelper::cloud_cb, &h, _1);
boost::signals2::connection c1 = interface->registerCallback (f);
interface->start ();
while (!p->wasStopped ())
{
p->spinOnce ();
if (new_cloud && mutex_.try_lock ())
{
new_cloud = false;
if (g_cloud)
{
FPS_CALC ("drawing");
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> handler (g_cloud);
if (!p->updatePointCloud (g_cloud, handler, "OpenNICloud"))
{
p->addPointCloud (g_cloud, handler, "OpenNICloud");
p->resetCameraViewpoint ("OpenNICloud");
}
}
mutex_.unlock ();
}
else
boost::this_thread::sleep (boost::posix_time::microseconds (1000));
}
interface->stop ();
}
void
cloud_cb (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & cloud)
{
uint64_t timestamp;
#ifdef USE_ROS
timestamp = cloud->header.stamp.toNSec() / 1000; //Microseconds
#else
timestamp = cloud->header.stamp;
#endif //USE_ROS
if (timestamp > 0)
PCL_INFO ("Acquired cloud with timestamp of %lu\n", timestamp);
if (mutex_.try_lock ())
{
cloud_ = cloud;
mutex_.unlock ();
}
}
void hpFrequentThread(
long start,
long stop,
std::vector<Conjunction> const& conjunctions,
std::map<Rule, OrderedCover> const& basicCovers,
OrderedCover const& trueCover,
double const treshold,
std::vector<Conjunction>& result,
boost::mutex& mutex
)
{
std::vector<Conjunction> _result;
_result.reserve(stop-start);
auto start_i = conjunctions.begin();
auto stop_i = conjunctions.begin();
std::advance(start_i, start);
std::advance(stop_i, stop);
long counter = 0;
for (auto i=start_i ; i!=stop_i ; ++i) {
OrderedCover _ordc(conjunctionCover(*i, basicCovers));
//auto xxx = _ordc.metrics(trueCover);
//printf("%d %d %d %d\n", xxx.tp(), xxx.fp(), xxx.tn(), xxx.fn());
//printf("%lf\n", _ordc.metrics(trueCover).fprate());
auto m = _ordc.metrics(trueCover);
// also require precision to be good enough as fprate is
// not reasonable metric anyway
if (m.fprate() <= treshold && m.precision() > 0) {
_result.push_back(*i);
counter += 1;
}
// try copying intermediate results
if (counter % 256 == 0) {
if (mutex.try_lock()) {
std::copy(_result.begin(), _result.end(),
std::back_inserter(result));
mutex.unlock();
_result.clear();
}
}
}
// copy to master _result vector
mutex.lock();
std::copy(_result.begin(), _result.end(), std::back_inserter(result));
mutex.unlock();
}
void runThread(int thread_id, boost::function<S()> threadFn, S& returnValue)
{
/* Uncomment line to delay first thread, useful to unearth errors/debug */
// if(thread_id == 0) { sleep(1); }
returnValue = threadFn();
if( mutex_done.try_lock() ) {
if(global_flag_done == false)
{
{
boost::lock_guard<boost::mutex> lock(mutex_main_wait);
global_winner = thread_id;
global_flag_done = true;
}
condition_var_main_wait.notify_all(); // we want main thread to quit
}
mutex_done.unlock();
}
}
////////////////////////////////////////////////////////////////////////////////
// high recall
////////////////////////////////////////////////////////////////////////////////
void hrFrequentThread(
long start,
long stop,
std::vector<Conjunction> const& conjunctions,
std::map<Rule, OrderedCover> const& basicCovers,
OrderedCover const& trueCover,
double const treshold,
std::vector<Conjunction>& result,
boost::mutex& mutex
)
{
std::vector<Conjunction> _result;
_result.reserve(stop-start);
auto start_i = conjunctions.begin();
auto stop_i = conjunctions.begin();
std::advance(start_i, start);
std::advance(stop_i, stop);
long counter = 0;
for (auto i=start_i ; i!=stop_i ; ++i) {
OrderedCover _ordc(conjunctionCover(*i, basicCovers));
if (_ordc.metrics(trueCover).recall() >= treshold) {
_result.push_back(*i);
counter += 1;
}
// try copying intermediate results
if (counter % 256 == 0) {
if (mutex.try_lock()) {
std::copy(_result.begin(), _result.end(),
std::back_inserter(result));
mutex.unlock();
_result.clear();
}
}
}
// copy to master _result vector
mutex.lock();
std::copy(_result.begin(), _result.end(), std::back_inserter(result));
mutex.unlock();
}
void mainLoopPlain(pcl::visualization::PCLVisualizer *viewer, PointCloudT::Ptr cloud, bool &new_cloud_available_flag)
{
while (true)
{
if (new_cloud_available_flag && cloud_mutex.try_lock()) // if a new cloud is available
{
new_cloud_available_flag = false;
// 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");
viewer->spinOnce();
cloud_mutex.unlock();
}
}
}
void
run ()
{
pcl::Grabber* interface = new pcl::OpenNIGrabber ();
boost::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&OpenNIOrganizedMultiPlaneSegmentation::cloud_cb_, this, _1);
//make a viewer
pcl::PointCloud<PointT>::Ptr init_cloud_ptr (new pcl::PointCloud<PointT>);
viewer = cloudViewer (init_cloud_ptr);
boost::signals2::connection c = interface->registerCallback (f);
interface->start ();
unsigned char red [6] = {255, 0, 0, 255, 255, 0};
unsigned char grn [6] = { 0, 255, 0, 255, 0, 255};
unsigned char blu [6] = { 0, 0, 255, 0, 255, 255};
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.03f);
ne.setNormalSmoothingSize (20.0f);
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (10000);
mps.setAngularThreshold (0.017453 * 2.0); //3 degrees
mps.setDistanceThreshold (0.02); //2cm
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
size_t prev_models_size = 0;
char name[1024];
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
if (prev_cloud && cloud_mutex.try_lock ())
{
regions.clear ();
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
double normal_start = pcl::getTime ();
ne.setInputCloud (prev_cloud);
ne.compute (*normal_cloud);
double normal_end = pcl::getTime ();
std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;
double plane_extract_start = pcl::getTime ();
mps.setInputNormals (normal_cloud);
mps.setInputCloud (prev_cloud);
mps.segmentAndRefine (regions);
double plane_extract_end = pcl::getTime ();
std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << std::endl;
std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;
pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
if (!viewer->updatePointCloud<PointT> (prev_cloud, "cloud"))
viewer->addPointCloud<PointT> (prev_cloud, "cloud");
removePreviousDataFromScreen (prev_models_size);
//Draw Visualization
for (size_t i = 0; i < regions.size (); i++)
{
Eigen::Vector3f centroid = regions[i].getCentroid ();
Eigen::Vector4f model = regions[i].getCoefficients ();
pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
centroid[1] + (0.5f * model[1]),
centroid[2] + (0.5f * model[2]));
sprintf (name, "normal_%lu", i);
viewer->addArrow (pt2, pt1, 1.0, 0, 0, false, name);
contour->points = regions[i].getContour ();
sprintf (name, "plane_%02zu", i);
pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
viewer->addPointCloud (contour, color, name);
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
}
prev_models_size = regions.size ();
cloud_mutex.unlock ();
}
}
interface->stop ();
}
void startRecording(string PCDfilepath, string TRJfilepath, pcl::visualization::PCLVisualizer *viewer, PointCloudT::Ptr &cloud, float dist, bool& new_cloud_available_flag)
{
// Writer::startWriting();
while (flag3)
{
if (new_cloud_available_flag && cloud_mutex.try_lock()) // if a new cloud is available
{
new_cloud_available_flag = false;
// Perform people detection on the new cloud:
std::vector<pcl::people::PersonCluster<PointT> > clusters; // vector containing persons clusters
people_detector.setInputCloud(cloud);
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;
centroids_curr.clear();
for (std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
{
if (it->getPersonConfidence() > min_confidence) // draw only people with confidence above a threshold
{
Eigen::Vector3f centroid_coords; //vector containing persons centroid coordinates
centroid_coords = it->getCenter(); // calculate centroid coordinates
int person_on_screen; // person number displayed on screen
person_on_screen = if_same_person(centroids_prev, centroid_coords, dist); // check if current person existed in prev frame
// add coordinates to vector containing people from current frame
float x = centroid_coords[0]; // extract x coordinate of centroid
float y = centroid_coords[1]; // extract y coordinate of centroid
float z = centroid_coords[2]; // extract z coorfinate of centroid
PointT pp;
pp.getVector3fMap() = it->getCenter();
Eigen::Vector4f centroid_coords_person;
centroid_coords_person[0] = x;
centroid_coords_person[1] = y;
centroid_coords_person[2] = z;
centroid_coords_person[3] = (float)person_on_screen;
centroids_curr.push_back(centroid_coords_person);
// draw theoretical person bounding box in the PCL viewer:
it->drawTBoundingBox(*viewer, k); // draw persons bounding box
cout << "tesst";
//creating text to display near person box
string tekst = "person ";
string Result;
ostringstream convert;
convert << person_on_screen;
Result = convert.str();
tekst = tekst + Result;
viewer->addText3D(tekst, pp, 0.08); // display text
k++;
cout << "-------------";
}
}
if (k == 0)
{
empty_in_row++;
cout << "Empty in a row: " << empty_in_row;
}
else empty_in_row = 0;
if (empty_in_row == 3) {
cout << "Czyszcze wektor przechowujacy dane o postaciach";
centroids_prev.clear();
}
if (k > 0)centroids_prev = centroids_curr;
std::cout << k << " people found" << std::endl;
viewer->spinOnce();
// Writer::write(cloud, clusters);
cloud_mutex.unlock();
}
}
}
int main (int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
if (pcl::console::find_argument (argc, argv, "-h") >= 0)
{
printUsage (argv[0]);
return 0;
}
if (pcl::console::parse (argc, argv, "-d", device_id) >= 0)
cout << "Using device id \""<<device_id<<"\".\n";
if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
angular_resolution = pcl::deg2rad (angular_resolution);
pcl::visualization::RangeImageVisualizer range_image_widget ("Range Image");
pcl::visualization::PCLVisualizer viewer ("3D Viewer");
viewer.addCoordinateSystem (1.0f, "global");
viewer.setBackgroundColor (1, 1, 1);
// Set the viewing pose so that the openni cloud is visible
viewer.initCameraParameters ();
viewer.setCameraPosition (0.0, -0.3, -2.0,
0.0, -0.3, 1.0,
0.0, -1.0, 0.0);
openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
if (driver.getNumberDevices () > 0)
{
for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
{
cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx)
<< ", product: " << driver.getProductName (deviceIdx) << ", connected: "
<< (int) driver.getBus (deviceIdx) << " @ " << (int) driver.getAddress (deviceIdx)
<< ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'\n";
}
}
else
{
cout << "\nNo devices connected.\n\n";
return 1;
}
pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
EventHelper event_helper;
boost::function<void (const openni_wrapper::DepthImage::Ptr&) > f_depth_image =
boost::bind (&EventHelper::depth_image_cb, &event_helper, _1);
boost::signals2::connection c_depth_image = interface->registerCallback (f_depth_image);
cout << "Starting grabber\n";
interface->start ();
cout << "Done\n";
pcl::RangeImagePlanar::Ptr range_image_planar_ptr (new pcl::RangeImagePlanar);
pcl::RangeImagePlanar& range_image_planar = *range_image_planar_ptr;
while (!viewer.wasStopped ())
{
viewer.spinOnce (); // process 3D Viewer events
range_image_widget.spinOnce (); // process Image Viewer events
pcl_sleep (0.01);
bool got_new_range_image = false;
if (received_new_depth_data && depth_image_mutex.try_lock ())
{
received_new_depth_data = false;
int frame_id = depth_image_ptr->getFrameID ();
cout << "Visualizing frame "<<frame_id<<"\n";
const unsigned short* depth_map = depth_image_ptr->getDepthMetaData ().Data ();
int width = depth_image_ptr->getWidth (), height = depth_image_ptr->getHeight ();
float center_x = width/2, center_y = height/2;
float focal_length_x = depth_image_ptr->getFocalLength (), focal_length_y = focal_length_x;
float original_angular_resolution = asinf (0.5f*float (width)/float (focal_length_x)) / (0.5f*float (width));
float desired_angular_resolution = angular_resolution;
range_image_planar.setDepthImage (depth_map, width, height, center_x, center_y,
focal_length_x, focal_length_y, desired_angular_resolution);
depth_image_mutex.unlock ();
got_new_range_image = !range_image_planar.points.empty ();
}
if (!got_new_range_image)
continue;
// Show range image in the image widget
range_image_widget.showRangeImage (range_image_planar, 0.5f, 10.0f);
// Show range image in the 3D viewer
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud
(range_image_planar_ptr, 0, 0, 0);
if (!viewer.updatePointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image"))
viewer.addPointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image");
}
interface->stop ();
}
void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
if (!we_have_a_map)
{
ROS_INFO("SnapMapICP waiting for map to be published");
return;
}
ros::Time scan_in_time = scan_in->header.stamp;
ros::Time time_received = ros::Time::now();
if ( scan_in_time - last_processed_scan < ros::Duration(1.0f / SCAN_RATE) )
{
ROS_DEBUG("rejected scan, last %f , this %f", last_processed_scan.toSec() ,scan_in_time.toSec());
return;
}
//projector_.transformLaserScanToPointCloud("base_link",*scan_in,cloud,listener_);
if (!scan_callback_mutex.try_lock())
return;
ros::Duration scan_age = ros::Time::now() - scan_in_time;
//check if we want to accept this scan, if its older than 1 sec, drop it
if (!use_sim_time)
if (scan_age.toSec() > AGE_THRESHOLD)
{
//ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD);
ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold) scan time: %f , now %f", scan_age.toSec(), AGE_THRESHOLD, scan_in_time.toSec(),ros::Time::now().toSec() );
scan_callback_mutex.unlock();
return;
}
count_sc_++;
//ROS_DEBUG("count_sc %i MUTEX LOCKED", count_sc_);
//if (count_sc_ > 10)
//if (count_sc_ > 10)
{
count_sc_ = 0;
tf::StampedTransform base_at_laser;
if (!getTransform(base_at_laser, ODOM_FRAME, "base_link", scan_in_time))
{
ROS_WARN("Did not get base pose at laser scan time");
scan_callback_mutex.unlock();
return;
}
sensor_msgs::PointCloud cloud;
sensor_msgs::PointCloud cloudInMap;
projector_->projectLaser(*scan_in,cloud);
we_have_a_scan = false;
bool gotTransform = false;
if (!listener_->waitForTransform("/map", cloud.header.frame_id, cloud.header.stamp, ros::Duration(0.05)))
{
scan_callback_mutex.unlock();
ROS_WARN("SnapMapICP no map to cloud transform found MUTEX UNLOCKED");
return;
}
if (!listener_->waitForTransform("/map", "/base_link", cloud.header.stamp, ros::Duration(0.05)))
{
scan_callback_mutex.unlock();
ROS_WARN("SnapMapICP no map to base transform found MUTEX UNLOCKED");
return;
}
while (!gotTransform && (ros::ok()))
{
try
{
gotTransform = true;
listener_->transformPointCloud ("/map",cloud,cloudInMap);
}
catch (...)
{
gotTransform = false;
ROS_WARN("DIDNT GET TRANSFORM IN A");
}
}
for (size_t k =0; k < cloudInMap.points.size(); k++)
{
cloudInMap.points[k].z = 0;
}
gotTransform = false;
tf::StampedTransform oldPose;
while (!gotTransform && (ros::ok()))
{
try
{
gotTransform = true;
listener_->lookupTransform("/map", "/base_link",
cloud.header.stamp , oldPose);
}
catch (tf::TransformException ex)
{
gotTransform = false;
ROS_WARN("DIDNT GET TRANSFORM IN B");
}
}
if (we_have_a_map && gotTransform)
{
sensor_msgs::convertPointCloudToPointCloud2(cloudInMap,cloud2);
we_have_a_scan = true;
actScan++;
//pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> reg;
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> reg;
reg.setTransformationEpsilon (1e-6);
// Set the maximum distance between two correspondences (src<->tgt) to 10cm
// Note: adjust this based on the size of your datasets
reg.setMaxCorrespondenceDistance(0.5);
reg.setMaximumIterations (ICP_NUM_ITER);
// Set the point representation
//ros::Time bef = ros::Time::now();
PointCloudT::Ptr myMapCloud (new PointCloudT());
PointCloudT::Ptr myScanCloud (new PointCloudT());
pcl::fromROSMsg(*output_cloud,*myMapCloud);
pcl::fromROSMsg(cloud2,*myScanCloud);
reg.setInputCloud(myScanCloud);
reg.setInputTarget(myMapCloud);
PointCloudT unused;
int i = 0;
reg.align (unused);
const Eigen::Matrix4f &transf = reg.getFinalTransformation();
tf::Transform t;
matrixAsTransfrom(transf,t);
//ROS_ERROR("proc time %f", (ros::Time::now() - bef).toSec());
we_have_a_scan_transformed = false;
PointCloudT transformedCloud;
pcl::transformPointCloud (*myScanCloud, transformedCloud, reg.getFinalTransformation());
double inlier_perc = 0;
{
// count inliers
std::vector<int> nn_indices (1);
std::vector<float> nn_sqr_dists (1);
size_t numinliers = 0;
for (size_t k = 0; k < transformedCloud.points.size(); ++k )
{
if (mapTree->radiusSearch (transformedCloud.points[k], ICP_INLIER_DIST, nn_indices,nn_sqr_dists, 1) != 0)
numinliers += 1;
}
if (transformedCloud.points.size() > 0)
{
//ROS_INFO("Inliers in dist %f: %zu of %zu percentage %f (%f)", ICP_INLIER_DIST, numinliers, transformedCloud.points.size(), (double) numinliers / (double) transformedCloud.points.size(), ICP_INLIER_THRESHOLD);
inlier_perc = (double) numinliers / (double) transformedCloud.points.size();
}
}
last_processed_scan = scan_in_time;
pcl::toROSMsg (transformedCloud, cloud2transformed);
we_have_a_scan_transformed = true;
double dist = sqrt((t.getOrigin().x() * t.getOrigin().x()) + (t.getOrigin().y() * t.getOrigin().y()));
double angleDist = t.getRotation().getAngle();
tf::Vector3 rotAxis = t.getRotation().getAxis();
t = t * oldPose;
tf::StampedTransform base_after_icp;
if (!getTransform(base_after_icp, ODOM_FRAME, "base_link", ros::Time(0)))
{
ROS_WARN("Did not get base pose at now");
scan_callback_mutex.unlock();
return;
}
else
{
tf::Transform rel = base_at_laser.inverseTimes(base_after_icp);
ROS_DEBUG("relative motion of robot while doing icp: %fcm %fdeg", rel.getOrigin().length(), rel.getRotation().getAngle() * 180 / M_PI);
t= t * rel;
}
//ROS_DEBUG("dist %f angleDist %f",dist, angleDist);
//ROS_DEBUG("SCAN_AGE seems to be %f", scan_age.toSec());
char msg_c_str[2048];
sprintf(msg_c_str,"INLIERS %f (%f) scan_age %f (%f age_threshold) dist %f angleDist %f axis(%f %f %f) fitting %f (icp_fitness_threshold %f)",inlier_perc, ICP_INLIER_THRESHOLD, scan_age.toSec(), AGE_THRESHOLD ,dist, angleDist, rotAxis.x(), rotAxis.y(), rotAxis.z(),reg.getFitnessScore(), ICP_FITNESS_THRESHOLD );
std_msgs::String strmsg;
strmsg.data = msg_c_str;
//ROS_DEBUG("%s", msg_c_str);
double cov = POSE_COVARIANCE_TRANS;
//if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (angleDist < ANGLE_UPPER_THRESHOLD))
// if ( reg.getFitnessScore() <= ICP_FITNESS_THRESHOLD )
if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (inlier_perc > ICP_INLIER_THRESHOLD) && (angleDist < ANGLE_UPPER_THRESHOLD))
{
lastTimeSent = actScan;
geometry_msgs::PoseWithCovarianceStamped pose;
pose.header.frame_id = "map";
pose.pose.pose.position.x = t.getOrigin().x();
pose.pose.pose.position.y = t.getOrigin().y();
tf::Quaternion quat = t.getRotation();
//quat.setRPY(0.0, 0.0, theta);
tf::quaternionTFToMsg(quat,pose.pose.pose.orientation);
float factorPos = 0.03;
float factorRot = 0.1;
pose.pose.covariance[6*0+0] = (cov * cov) * factorPos;
pose.pose.covariance[6*1+1] = (cov * cov) * factorPos;
pose.pose.covariance[6*3+3] = (M_PI/12.0 * M_PI/12.0) * factorRot;
ROS_DEBUG("i %i converged %i SCORE: %f", i, reg.hasConverged (), reg.getFitnessScore() );
ROS_DEBUG("PUBLISHING A NEW INITIAL POSE dist %f angleDist %f Setting pose: %.3f %.3f [frame=%s]",dist, angleDist , pose.pose.pose.position.x , pose.pose.pose.position.y , pose.header.frame_id.c_str());
pub_pose.publish(pose);
strmsg.data += " << SENT";
}
//ROS_INFO("processing time : %f", (ros::Time::now() - time_received).toSec());
pub_info_.publish(strmsg);
//ROS_DEBUG("map width %i height %i size %i, %s", myMapCloud.width, myMapCloud.height, (int)myMapCloud.points.size(), myMapCloud.header.frame_id.c_str());
//ROS_DEBUG("scan width %i height %i size %i, %s", myScanCloud.width, myScanCloud.height, (int)myScanCloud.points.size(), myScanCloud.header.frame_id.c_str());
}
}
scan_callback_mutex.unlock();
}
void Run()
{ //cout << "here0" << endl;
int w, h;
// Create camera instances
_cam = CLEyeCreateCamera(_cameraGUID, _mode, _resolution, _fps);
if(_cam == NULL) return;
// Get camera frame dimensions
CLEyeCameraGetFrameDimensions(_cam, w, h);
IplImage *pCapImage;
// Create the OpenCV images
pCapImage = cvCreateImage(cvSize(w, h), IPL_DEPTH_8U, _isColor ? 4 : 1);
// Set some camera parameters
CLEyeSetCameraParameter(_cam, CLEYE_GAIN, 0);
CLEyeSetCameraParameter(_cam, CLEYE_EXPOSURE, 511);
// Start capturing
CLEyeCameraStart(_cam);
Sleep(100);
tMin = 1000;
tMax = 0;
tAvg = 0;
curr = 0;
measuredCnt = 0;
printf("\n");
//cout << "here1" << endl;
// Capture camera images
PBYTE tempBuffer;
cvGetImageRawData(pCapImage, &tempBuffer);
CLEyeCameraGetFrame(_cam, tempBuffer);
Mat imgTemp(pCapImage);
mtx.lock();
imgLines = Mat::zeros( imgTemp.size(), CV_8UC4 );
imgCapture = Mat::zeros( imgTemp.size(), CV_8UC4 );
mtx.unlock();
//cout << "here2" << endl;
// image capturing loop
while(_running)
{
//oldFrameTimestamp = frameTimestamp;
//oldFrameTimestamp = getTickCount();
//cout << "here3" << endl;
PBYTE pCapBuffer;
// Capture camera images
cvGetImageRawData(pCapImage, &pCapBuffer);
CLEyeCameraGetFrame(_cam, pCapBuffer);
//IplImage* imgHSV = cvCreateImage(cvGetSize(pCapImage), IPL_DEPTH_8U, 3);
//cvCvtColor(pCapImage, imgHSV, CV_BGR2HSV); //Change the color format from BGR to HSV
//IplImage* imgThresholded = cvCreateImage(cvGetSize(imgHSV),IPL_DEPTH_8U, 1);
//cvInRangeS(imgHSV, cvScalar(minH,minS,minV), cvScalar(maxH,maxS,maxV), imgThresholded);
Mat imgCap(pCapImage); //creates 8UC4 Mat
mtx.try_lock();
imgCap.copyTo(imgCapture); //copy to shared variable
mtx.unlock();
//frameTimestamp = getTickCount();
//double t = ((double)(frameTimestamp - oldFrameTimestamp))/getTickFrequency() * 1000;
//cout << "Frame interval: " << t << endl;
}
Sleep(1000);
// Stop camera capture
CLEyeCameraStop(_cam);
// Destroy camera object
CLEyeDestroyCamera(_cam);
// Destroy the allocated OpenCV image
cvReleaseImage(&pCapImage);
_cam = NULL;
}
int main(int argc, char** argv)
{
int total_frame = 0;
ros::init(argc, argv, "pub_pcl");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<PointCloudT>("/openni/points2", 1);
// Read
PointCloudT::Ptr cloud(new PointCloudT);
bool new_cloud_avaliable_flag = false;
pcl::Grabber* interface = new pcl::OpenNIGrabber();
boost::function<void (const PointCloudT::ConstPtr&)> f = boost::bind(&cloud_cb_, _1, cloud, &new_cloud_avaliable_flag);
interface->registerCallback(f);
interface->start();
// Wait for the first frame
while (!new_cloud_avaliable_flag)
{
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
cloud_mutex.lock(); // for not overwriting the point cloud
// Display
printf("frame %d\n", ++total_frame);
pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
viewer.addPointCloud<PointT>(cloud, rgb, "input_cloud");
viewer.setCameraPosition(0, 0, -2, 0, -1, 0, 0);
cloud_mutex.unlock();
ros::Rate loop_rate(4);
while (nh.ok())
{
ros::Time scan_time = ros::Time::now();
// Get & publish new cloud
if (new_cloud_avaliable_flag && cloud_mutex.try_lock())
{
new_cloud_avaliable_flag = false;
pub.publish(cloud);
printf("frame %d\n", ++total_frame);
viewer.removeAllPointClouds();
viewer.removeAllShapes();
pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
viewer.addPointCloud<PointT>(cloud, rgb, "input_cloud");
cloud_mutex.unlock();
}
else
{
printf("no %d\n", total_frame);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
bool try_lock() { return m.try_lock(); }
/* ---[ */
int
main (int argc, char** argv)
{
srand (unsigned (time (0)));
if (argc > 1)
{
for (int i = 1; i < argc; i++)
{
if (std::string (argv[i]) == "-h")
{
printHelp (argc, argv);
return (-1);
}
}
}
// Command line parsing
double bcolor[3] = {0, 0, 0};
pcl::console::parse_3x_arguments (argc, argv, "-bc", bcolor[0], bcolor[1], bcolor[2]);
fcolorparam = pcl::console::parse_multiple_3x_arguments (argc, argv, "-fc", fcolor_r, fcolor_g, fcolor_b);
int psize = 0;
pcl::console::parse_argument (argc, argv, "-ps", psize);
double opaque;
pcl::console::parse_argument (argc, argv, "-opaque", opaque);
cloud_viewer.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer"));
#ifdef DISPLAY_IMAGE
img_viewer.reset (new pcl::visualization::ImageViewer ("OpenNI Viewer"));
#endif
// // Change the cloud rendered point size
// if (psize > 0)
// p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, "OpenNICloud");
//
// // Change the cloud rendered opacity
// if (opaque >= 0)
// p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque, "OpenNICloud");
cloud_viewer->setBackgroundColor (bcolor[0], bcolor[1], bcolor[2]);
// Read axes settings
double axes = 0.0;
pcl::console::parse_argument (argc, argv, "-ax", axes);
if (axes != 0.0 && cloud_viewer)
{
float ax_x = 0.0, ax_y = 0.0, ax_z = 0.0;
pcl::console::parse_3x_arguments (argc, argv, "-ax_pos", ax_x, ax_y, ax_z, false);
// Draw XYZ axes if command-line enabled
cloud_viewer->addCoordinateSystem (axes, ax_x, ax_y, ax_z);
}
float frames_per_second = 0; // 0 means only if triggered!
pcl::console::parse (argc, argv, "-fps", frames_per_second);
if (frames_per_second < 0)
frames_per_second = 0.0;
bool repeat = (pcl::console::find_argument (argc, argv, "-repeat") != -1);
bool use_pclzf = (pcl::console::find_argument (argc, argv, "-pclzf") != -1);
std::cout << "fps: " << frames_per_second << " , repeat: " << repeat << std::endl;
std::string path = "";
pcl::console::parse_argument (argc, argv, "-dir", path);
std::cout << "path: " << path << std::endl;
if (path != "" && boost::filesystem::exists (path))
{
grabber.reset (new pcl::ImageGrabber<pcl::PointXYZRGBA> (path, frames_per_second, repeat, use_pclzf));
}
else
{
std::cout << "No directory was given with the -dir flag." << std::endl;
printHelp (argc, argv);
return (-1);
}
grabber->setDepthImageUnits (float (1E-3));
// Before manually setting
double fx, fy, cx, cy;
grabber->getCameraIntrinsics (fx, fy, cx, cy);
PCL_INFO ("Factory default intrinsics: %f, %f, %f, %f\n",
fx, fy, cx, cy);
float focal_length;
if (pcl::console::parse (argc, argv, "-focal", focal_length) != -1)
grabber->setCameraIntrinsics (focal_length, focal_length, 320, 240);
EventHelper h;
boost::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&) > f = boost::bind (&EventHelper::cloud_cb, &h, _1);
boost::signals2::connection c1 = grabber->registerCallback (f);
std::string mouse_msg_3D ("Mouse coordinates in PCL Visualizer");
std::string key_msg_3D ("Key event for PCL Visualizer");
cloud_viewer->registerMouseCallback (&mouse_callback, static_cast<void*> (&mouse_msg_3D));
cloud_viewer->registerKeyboardCallback(&keyboard_callback, static_cast<void*> (&key_msg_3D));
std::string mouse_msg_2D ("Mouse coordinates in image viewer");
std::string key_msg_2D ("Key event for image viewer");
#ifdef DISPLAY_IMAGE
img_viewer->registerMouseCallback (&mouse_callback, static_cast<void*> (&mouse_msg_2D));
img_viewer->registerKeyboardCallback(&keyboard_callback, static_cast<void*> (&key_msg_2D));
#endif
grabber->start ();
grabber->getCameraIntrinsics (fx, fy, cx, cy);
PCL_INFO ("Grabber is using intrinsics: %f, %f, %f, %f\n",
fx, fy, cx, cy);
while (!cloud_viewer->wasStopped ())
{
cloud_viewer->spinOnce ();
#ifdef DISPLAY_IMAGE
img_viewer->spinOnce ();
#endif
if (!cloud_)
{
boost::this_thread::sleep(boost::posix_time::microseconds(10000));
continue;
}
else if (mutex_.try_lock ())
{
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr temp_cloud;
temp_cloud.swap (cloud_);
mutex_.unlock ();
#ifdef DISPLAY_IMAGE
img_viewer->showRGBImage (*temp_cloud);
#endif
if (!cloud_viewer->updatePointCloud (temp_cloud, "PCDCloud"))
{
cloud_viewer->addPointCloud (temp_cloud, "PCDCloud");
cloud_viewer->setCameraPosition (0, 0, 0, 0, 0, 1, 0, -1, 0);
}
}
}
grabber->stop ();
}
inline void Lock_Wait( boost::mutex& m )
{
while( m.try_lock() == false ){}
}
int
main (int argc, char **argv)
{
std::cout << "main function" << std::endl;
pcl::Grabber* interface;
ros::NodeHandle *nh;
ros::Subscriber sub;
ros::Publisher clusterPub, normalPub, planePub,msgPub;
bool spawnObject = true;
K2G *k2g;
processor freenectprocessor = OPENGL;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud;
std::cout << "ros node initialized" << std::endl;
ros::init(argc, argv, "hlpr_single_plane_segmentation",ros::init_options::NoSigintHandler);
nh = new ros::NodeHandle("~");
// nh->getParam("segmentation/viz", viz_);
// std::cout<<"viz is set to " << viz_ << endl;
parsedArguments pA;
if(parseArguments(argc, argv, pA, *nh) < 0)
return 0;
viz_ = pA.viz;
ridiculous_global_variables::ignore_low_sat = pA.saturation_hack;
ridiculous_global_variables::saturation_threshold = pA.saturation_hack_value;
ridiculous_global_variables::saturation_mapped_value = pA.saturation_mapped_value;
RansacSinglePlaneSegmentation multi_plane_app;
pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr ncloud_ptr (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Label>::Ptr label_ptr (new pcl::PointCloud<pcl::Label>);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
multi_plane_app.initSegmentation(pA.seg_color_ind, pA.ecc_dist_thresh, pA.ecc_color_thresh);
if(viz_) {
viewer = cloudViewer(cloud_ptr);
multi_plane_app.setViewer(viewer);
}
float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0};
multi_plane_app.setWorkingVolumeThresholds(workSpace);
msgPub = nh->advertise<hlpr_perception_msgs::SegClusters>(segOutRostopic,5);
switch (pA.pc_source)
{
case 0:
{
std::cout << "Using ros topic as input" << std::endl;
sub = nh->subscribe<sensor_msgs::PointCloud2>(pA.ros_topic, 1, cloud_cb_ros_);
break;
}
case 1:
{
std::cout << "Using the openni device" << std::endl;
interface = new pcl::OpenNIGrabber ();
boost::function<void(const pcl::PointCloud<PointT>::ConstPtr&)> f = boost::bind (&cloud_cb_direct_,_1);
boost::signals2::connection c = interface->registerCallback (f);
interface->start ();
break;
}
case 2:
default:
{
std::cout << "Using kinect v2" << std::endl;
freenectprocessor = static_cast<processor>(pA.freenectProcessor);
k2g = new K2G(freenectprocessor, true);
cloud = k2g->getCloud();
prev_cloud = cloud;
gotFirst = true;
break;
}
}
signal(SIGINT, interruptFn);
while (!interrupt && (!viz_ || !viewer->wasStopped ()))
{
if(pA.ros_node)
{
ros::spinOnce();
}
if(viz_)
viewer->spinOnce(20);
if(!gotFirst)
continue;
int selected_cluster_index = -1;
if(cloud_mutex.try_lock ())
{
if(pA.pc_source == 2)
{
cloud = k2g->getCloud();
fake_cloud_cb_kinectv2_(cloud);
}
if(viz_ && pA.justViewPointCloud)
{
pcl::PointCloud<PointT>::Ptr filtered_prev_cloud(new pcl::PointCloud<PointT>(*prev_cloud));
multi_plane_app.preProcPointCloud(filtered_prev_cloud);
if (!viewer->updatePointCloud<PointT> (filtered_prev_cloud, "cloud"))
{
viewer->addPointCloud<PointT> (filtered_prev_cloud, "cloud");
}
selected_cluster_index = -1;
}
else
{
pcl::PointCloud<PointT>::CloudVectorType clusters;
std::vector<pcl::PointCloud<pcl::Normal> > clusterNormals;
Eigen::Vector4f plane;
std::vector<size_t> clusterIndices;
std::vector<std::vector<int>> clusterIndicesStore;
selected_cluster_index = multi_plane_app.processOnce(prev_cloud,clusters,clusterNormals,plane,clusterIndicesStore,
pA.pre_proc,
pA.merge_clusters, viz_, pA.filterNoise); //true is for the viewer
hlpr_perception_msgs::SegClusters msg;
//msg.header.stamp = ros::Time::now();
// Pull out the cluster indices and put in msg
for (int ti = 0; ti < clusterIndicesStore.size(); ti++){
hlpr_perception_msgs::ClusterIndex cluster_idx_msg;
for (int j = 0; j < clusterIndicesStore[ti].size(); j++){
std_msgs::Int32 temp_msg;
temp_msg.data = clusterIndicesStore[ti][j];
cluster_idx_msg.indices.push_back(temp_msg);
}
msg.cluster_ids.push_back(cluster_idx_msg);
//clusterIndices.insert(clusterIndices.end(),clusterIndicesStore[ti].begin(), clusterIndicesStore[ti].end()); // For removing ALL cluster points
}
for(int i = 0; i < clusters.size(); i++) {
sensor_msgs::PointCloud2 out;
pcl::PCLPointCloud2 tmp;
pcl::toPCLPointCloud2(clusters[i], tmp);
pcl_conversions::fromPCL(tmp, out);
msg.clusters.push_back(out);
}
for(int i = 0; i < clusterNormals.size(); i++) {
sensor_msgs::PointCloud2 out;
pcl::PCLPointCloud2 tmp;
pcl::toPCLPointCloud2(clusterNormals[i], tmp);
pcl_conversions::fromPCL(tmp, out);
msg.normals.push_back(out);
}
std_msgs::Float32MultiArray planeMsg;
planeMsg.data.clear();
for(int i = 0; i < 4; i++)
planeMsg.data.push_back(plane[i]);
//planePub.publish(planeMsg);
msg.plane = planeMsg;
msgPub.publish(msg);
//clusterPub.publish(clusters[0]);
//normalPub.publish(clusterNormals[0]);
}
cloud_mutex.unlock();
/*the z_thresh may result in wrong cluster to be selected. it might be a good idea to do
* some sort of mean shift tracking, or selecting the biggest amongst the candidates (vs choosing the most similar color)
* or sending all the plausible ones to c6 and decide there
*/
}
if(selected_cluster_index < 0)
continue;
}
if(pA.pc_source == 1)
{
interface->stop ();
delete interface;
}
delete nh;
ros::shutdown();
return 0;
}
/* ---[ */
int
main (int argc, char** argv)
{
srand (unsigned (time (0)));
if (argc > 1)
{
for (int i = 1; i < argc; i++)
{
if (std::string (argv[i]) == "-h")
{
printHelp (argc, argv);
return (-1);
}
}
}
// Command line parsing
double bcolor[3] = {0, 0, 0};
pcl::console::parse_3x_arguments (argc, argv, "-bc", bcolor[0], bcolor[1], bcolor[2]);
fcolorparam = pcl::console::parse_multiple_3x_arguments (argc, argv, "-fc", fcolor_r, fcolor_g, fcolor_b);
int psize = 0;
pcl::console::parse_argument (argc, argv, "-ps", psize);
double opaque;
pcl::console::parse_argument (argc, argv, "-opaque", opaque);
cloud_viewer.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer"));
#if !((VTK_MAJOR_VERSION == 5)&&(VTK_MINOR_VERSION <= 4))
img_viewer.reset (new pcl::visualization::ImageViewer ("OpenNI Viewer"));
#endif
// // Change the cloud rendered point size
// if (psize > 0)
// p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, "OpenNICloud");
//
// // Change the cloud rendered opacity
// if (opaque >= 0)
// p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque, "OpenNICloud");
cloud_viewer->setBackgroundColor (bcolor[0], bcolor[1], bcolor[2]);
// Read axes settings
double axes = 0.0;
pcl::console::parse_argument (argc, argv, "-ax", axes);
if (axes != 0.0 && cloud_viewer)
{
float ax_x = 0.0, ax_y = 0.0, ax_z = 0.0;
pcl::console::parse_3x_arguments (argc, argv, "-ax_pos", ax_x, ax_y, ax_z, false);
// Draw XYZ axes if command-line enabled
cloud_viewer->addCoordinateSystem (axes, ax_x, ax_y, ax_z);
}
float frames_per_second = 0; // 0 means only if triggered!
pcl::console::parse (argc, argv, "-fps", frames_per_second);
if (frames_per_second < 0)
frames_per_second = 0.0;
bool repeat = (pcl::console::find_argument (argc, argv, "-repeat") != -1);
std::cout << "fps: " << frames_per_second << " , repeat: " << repeat << std::endl;
std::string path = "";
pcl::console::parse_argument (argc, argv, "-file", path);
std::cout << "path: " << path << std::endl;
if (path != "" && boost::filesystem::exists (path))
{
grabber.reset (new pcl::PCDGrabber<pcl::PointXYZRGB> (path, frames_per_second, repeat));
}
else
{
std::vector<std::string> pcd_files;
pcl::console::parse_argument (argc, argv, "-dir", path);
std::cout << "path: " << path << std::endl;
if (path != "" && boost::filesystem::exists (path))
{
boost::filesystem::directory_iterator end_itr;
for (boost::filesystem::directory_iterator itr (path); itr != end_itr; ++itr)
{
if (!is_directory (itr->status()) && boost::algorithm::to_upper_copy(boost::filesystem::extension (itr->leaf())) == ".PCD" )
{
pcd_files.push_back (itr->path ().string());
std::cout << "added: " << itr->path ().string() << std::endl;
}
}
}
else
{
std::cout << "Neither a pcd file given using the \"-file\" option, nor given a directory containing pcd files using the \"-dir\" option." << std::endl;
}
// Sort the read files by name
sort (pcd_files.begin (), pcd_files.end ());
grabber.reset (new pcl::PCDGrabber<pcl::PointXYZRGB> (pcd_files, frames_per_second, repeat));
}
EventHelper h;
boost::function<void(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&) > f = boost::bind (&EventHelper::cloud_cb, &h, _1);
boost::signals2::connection c1 = grabber->registerCallback (f);
std::string mouseMsg3D ("Mouse coordinates in PCL Visualizer");
std::string keyMsg3D ("Key event for PCL Visualizer");
cloud_viewer->registerMouseCallback (&mouse_callback, reinterpret_cast<void*> (&mouseMsg3D));
cloud_viewer->registerKeyboardCallback(&keyboard_callback, reinterpret_cast<void*> (&keyMsg3D));
grabber->start ();
while (!cloud_viewer->wasStopped ())
{
cloud_viewer->spinOnce ();
#if !((VTK_MAJOR_VERSION == 5)&&(VTK_MINOR_VERSION <= 4))
img_viewer->spinOnce ();
#endif
if (!cloud_)
{
boost::this_thread::sleep(boost::posix_time::microseconds(10000));
continue;
}
if (mutex_.try_lock ())
{
pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr temp_cloud;
temp_cloud.swap (cloud_);
mutex_.unlock ();
#if !((VTK_MAJOR_VERSION == 5)&&(VTK_MINOR_VERSION <= 4))
img_viewer->showRGBImage (*temp_cloud);
#endif
if (!cloud_viewer->updatePointCloud (temp_cloud, "PCDCloud"))
{
cloud_viewer->addPointCloud (temp_cloud, "PCDCloud");
cloud_viewer->resetCameraViewpoint ("PCDCloud");
}
}
}
grabber->stop ();
}
/* ---[ */
int
main (int argc, char** argv)
{
if (argc > 1)
{
for (int i = 1; i < argc; i++)
{
if (std::string (argv[i]) == "-h")
{
printHelp (argc, argv);
return (-1);
}
}
}
EventHelper event_helper;
std::string device_id = "";
pcl::console::parse_argument (argc, argv, "-dev", device_id);
pcl::Grabber* grabber = new pcl::OpenNIGrabber (device_id);
cld.reset (new pcl::visualization::PCLVisualizer (argc, argv, "OpenNI Viewer"));
cld->setBackgroundColor(255,255,255);//背景を白色（255,255,255）に。
std::string mouseMsg3D ("Mouse coordinates in PCL Visualizer");
std::string keyMsg3D ("Key event for PCL Visualizer");
cld->registerMouseCallback (&mouse_callback, (void*)(&mouseMsg3D));
cld->registerKeyboardCallback(&keyboard_callback, (void*)(&keyMsg3D));
boost::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&) > f = boost::bind (&EventHelper::cloud_cb, &event_helper, _1);
boost::signals2::connection c1 = grabber->registerCallback (f);
#if !((VTK_MAJOR_VERSION == 5)&&(VTK_MINOR_VERSION <= 4))
img.reset (new pcl::visualization::ImageViewer ("OpenNI Viewer"));
// Register callbacks
std::string keyMsg2D ("Key event for image viewer");
std::string mouseMsg2D ("Mouse coordinates in image viewer");
img->registerMouseCallback (&mouse_callback, (void*)(&mouseMsg2D));
img->registerKeyboardCallback(&keyboard_callback, (void*)(&keyMsg2D));
boost::function<void (const boost::shared_ptr<openni_wrapper::Image>&) > image_cb = boost::bind (&EventHelper::image_callback, &event_helper, _1);
boost::signals2::connection image_connection = grabber->registerCallback (image_cb);
unsigned char* rgb_data = 0;
unsigned rgb_data_size = 0;
#endif
grabber->start ();
bool cld_init = false;
// Loop
while (!cld->wasStopped ())
{
// Render and process events in the two interactors
cld->spinOnce ();
#if !((VTK_MAJOR_VERSION == 5)&&(VTK_MINOR_VERSION <= 4))
img->spinOnce ();
#endif
// Add the cloud
if (g_cloud && cld_mutex.try_lock ())
{
if (!cld_init)
{
cld->getRenderWindow ()->SetSize (g_cloud->width, g_cloud->height);
cld->getRenderWindow ()->SetPosition (g_cloud->width, 0);
cld_init = !cld_init;
}
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> handler (g_cloud);
if (!cld->updatePointCloud (g_cloud, handler, "OpenNICloud"))
{
cld->addPointCloud (g_cloud, handler, "OpenNICloud");
cld->resetCameraViewpoint ("OpenNICloud");
}
cld_mutex.unlock ();
}
#if !((VTK_MAJOR_VERSION == 5)&&(VTK_MINOR_VERSION <= 4))
// Add the image
if (g_image && img_mutex.try_lock ())
{
if (g_image->getEncoding() == openni_wrapper::Image::RGB)
img->showRGBImage (g_image->getMetaData ().Data (),
g_image->getWidth (), g_image->getHeight ());
else
{
if (rgb_data_size < g_image->getWidth () * g_image->getHeight ())
{
rgb_data_size = g_image->getWidth () * g_image->getHeight ();
rgb_data = new unsigned char [rgb_data_size * 3];
}
g_image->fillRGB (g_image->getWidth (), g_image->getHeight (), rgb_data);
img->showRGBImage (rgb_data, g_image->getWidth (), g_image->getHeight ());
}
img_mutex.unlock ();
}
#endif
boost::this_thread::sleep (boost::posix_time::microseconds (100));
}
grabber->stop ();
}
BOOLEAN try_get ()
{
return (BOOLEAN) _lock.try_lock () ;
}
int main (int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
if (pcl::console::find_argument (argc, argv, "-h") >= 0)
{
printUsage (argv[0]);
return 0;
}
if (pcl::console::parse (argc, argv, "-d", device_id) >= 0)
cout << "Using device id \""<<device_id<<"\".\n";
if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
angular_resolution = pcl::deg2rad (angular_resolution);
if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
cout << "Setting support size to "<<support_size<<"m.\n";
if (pcl::console::parse (argc, argv, "-i", min_interest_value) >= 0)
cout << "Setting minimum interest value to "<<min_interest_value<<".\n";
if (pcl::console::parse (argc, argv, "-t", max_no_of_threads) >= 0)
cout << "Setting maximum number of threads to "<<max_no_of_threads<<".\n";
pcl::visualization::RangeImageVisualizer range_image_widget ("Range Image");
pcl::visualization::PCLVisualizer viewer ("3D Viewer");
viewer.addCoordinateSystem (1.0f);
viewer.setBackgroundColor (1, 1, 1);
viewer.initCameraParameters ();
viewer.camera_.pos[0] = 0.0;
viewer.camera_.pos[1] = -0.3;
viewer.camera_.pos[2] = -2.0;
viewer.camera_.focal[0] = 0.0;
viewer.camera_.focal[1] = 0.0+viewer.camera_.pos[1];
viewer.camera_.focal[2] = 1.0;
viewer.camera_.view[0] = 0.0;
viewer.camera_.view[1] = -1.0;
viewer.camera_.view[2] = 0.0;
viewer.updateCamera ();
openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
if (driver.getNumberDevices () > 0)
{
for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
{
cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx)
<< ", product: " << driver.getProductName (deviceIdx) << ", connected: "
<< (int) driver.getBus (deviceIdx) << " @ " << (int) driver.getAddress (deviceIdx)
<< ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'\n";
}
}
else
{
cout << "\nNo devices connected.\n\n";
return 1;
}
pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
EventHelper event_helper;
boost::function<void (const boost::shared_ptr<openni_wrapper::DepthImage>&) > f_depth_image =
boost::bind (&EventHelper::depth_image_cb, &event_helper, _1);
boost::signals2::connection c_depth_image = interface->registerCallback (f_depth_image);
cout << "Starting grabber\n";
interface->start ();
cout << "Done\n";
boost::shared_ptr<pcl::RangeImagePlanar> range_image_planar_ptr (new pcl::RangeImagePlanar);
pcl::RangeImagePlanar& range_image_planar = *range_image_planar_ptr;
pcl::RangeImageBorderExtractor range_image_border_extractor;
pcl::NarfKeypoint narf_keypoint_detector;
narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor);
narf_keypoint_detector.getParameters ().support_size = support_size;
narf_keypoint_detector.getParameters ().max_no_of_threads = max_no_of_threads;
narf_keypoint_detector.getParameters ().min_interest_value = min_interest_value;
//narf_keypoint_detector.getParameters ().calculate_sparse_interest_image = false;
//narf_keypoint_detector.getParameloadters ().add_points_on_straight_edges = true;
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>& keypoints_cloud = *keypoints_cloud_ptr;
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
range_image_widget.spinOnce (); // process GUI events
pcl_sleep (0.01);
bool got_new_range_image = false;
if (received_new_depth_data && depth_image_mutex.try_lock ())
{
received_new_depth_data = false;
//unsigned long time_stamp = depth_image_ptr->getTimeStamp ();
//int frame_id = depth_image_ptr->getFrameID ();
const unsigned short* depth_map = depth_image_ptr->getDepthMetaData ().Data ();
int width = depth_image_ptr->getWidth (), height = depth_image_ptr->getHeight ();
float center_x = width/2, center_y = height/2;
float focal_length_x = depth_image_ptr->getFocalLength (), focal_length_y = focal_length_x;
float original_angular_resolution = asinf (0.5f*float (width)/float (focal_length_x)) / (0.5f*float (width));
float desired_angular_resolution = angular_resolution;
range_image_planar.setDepthImage (depth_map, width, height, center_x, center_y,
focal_length_x, focal_length_y, desired_angular_resolution);
depth_image_mutex.unlock ();
got_new_range_image = !range_image_planar.points.empty ();
}
if (!got_new_range_image)
continue;
// --------------------------------
// -----Extract NARF keypoints-----
// --------------------------------
if (set_unseen_to_max_range)
range_image_planar.setUnseenToMaxRange ();
narf_keypoint_detector.setRangeImage (&range_image_planar);
pcl::PointCloud<int> keypoint_indices;
double keypoint_extraction_start_time = pcl::getTime();
narf_keypoint_detector.compute (keypoint_indices);
double keypoint_extraction_time = pcl::getTime()-keypoint_extraction_start_time;
std::cout << "Found "<<keypoint_indices.points.size ()<<" key points. "
<< "This took "<<1000.0*keypoint_extraction_time<<"ms.\n";
// ----------------------------------------------
// -----Show keypoints in range image widget-----
// ----------------------------------------------
range_image_widget.showRangeImage (range_image_planar, 0.5f, 10.0f);
//for (size_t i=0; i<keypoint_indices.points.size (); ++i)
//range_image_widget.markPoint (keypoint_indices.points[i]%range_image_planar.width,
//keypoint_indices.points[i]/range_image_planar.width,
//pcl::visualization::Vector3ub (0,255,0));
// -------------------------------------
// -----Show keypoints in 3D viewer-----
// -------------------------------------
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud
(range_image_planar_ptr, 0, 0, 0);
if (!viewer.updatePointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image"))
viewer.addPointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image");
keypoints_cloud.points.resize (keypoint_indices.points.size ());
for (size_t i=0; i<keypoint_indices.points.size (); ++i)
keypoints_cloud.points[i].getVector3fMap () =
range_image_planar.points[keypoint_indices.points[i]].getVector3fMap ();
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_handler_keypoints
(keypoints_cloud_ptr, 0, 255, 0);
if (!viewer.updatePointCloud (keypoints_cloud_ptr, color_handler_keypoints, "keypoints"))
viewer.addPointCloud (keypoints_cloud_ptr, color_handler_keypoints, "keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
}
interface->stop ();
}
int _tmain(int argc, _TCHAR* argv[])
{
// Launch camera polling thread //
CLEyeCapture *cam = NULL;
// Query for number of connected cameras
int numCams = CLEyeGetCameraCount();
if(numCams == 0) {
printf("No PS3Eye cameras detected\n");
Sleep(1000);
return -1;
}
// Open serial port
ArduinoSerialInterface asInterface = *new ArduinoSerialInterface(L"\\\\.\\COM4");
bool opened = asInterface.openComPort();
if(!opened) {
cout << "Failed to open serial port!" << endl;
Sleep(1000);
return -1;
}
// Initialize camera output windows
if(displayEnabled) {
cvNamedWindow(OUTPUT_WINDOW, CV_WINDOW_AUTOSIZE);
cvNamedWindow("Contours",CV_WINDOW_AUTOSIZE);
}
// start capture
cam = new CLEyeCapture(CLEYE_QVGA, (CLEyeCameraColorMode)CLEYE_COLOR_PROCESSED, 100);
cam->Start();
// logging
ofstream myfile;
if(loggingEnabled) { myfile.open("asdf.txt"); }
Sleep(2000); //wait for camera to initialize -- this could end badly
ofstream debugInfo;
if (writeFrameInfo) { debugInfo.open("log.txt", std::ofstream::out); debugInfo << setprecision(9); }
// puck HSV bounds
PuckTracker::HSVBounds pbounds;
pbounds.minH=21; pbounds.maxH=72;
pbounds.minS=47; pbounds.maxS=106;
pbounds.minV=163; pbounds.maxV=255;
// robot HSV bounds
PuckTracker::HSVBounds rbounds;
rbounds.minH=0; rbounds.maxH=28;
rbounds.minS=123; rbounds.maxS=211;
rbounds.minV=160; rbounds.maxV=255;
// Initialize puckTracker class -- maintains puck state variables
float rPuck = 1.60, rPaddle = 1.99; //paddle radius, inches
PuckTracker puckTracker(rPuck, pbounds);
//PuckTracker robotTracker(rPuck, rbounds);
int colorCycler = 0;
// set up local state variables to point to pucktracker vars
//float xPC, yPC, xPT, yPT, vPx, vPy;
float &xPC = puckTracker.xPC, &yPC = puckTracker.yPC, &xPT = puckTracker.xPT, &yPT = puckTracker.yPT;
float &vPx = puckTracker.vPx, &vPy = puckTracker.vPy;
// x,y,t are target defense position (defense) or target puck position (attack). angleTarget is angle of attack
float xDef = 0, yDef = 0, tDef = 0, xAtt = 0, yAtt = 0, tAtt = 0, xAtt2 = 0, yAtt2 = 0, tAtt2 = 0;
deque<float> yDefArr, tDefArr, yAttArr, tAttArr, yAtt2Arr, tAtt2Arr;
float critXvalues[] = { 23, 15, 8.75, 6, 3}; //x locations at which trajectory is evaluated for AI decision-making
float critYvalues[numberOfCritXvalues], critTvalues[numberOfCritXvalues], critVyvalues[numberOfCritXvalues];
float tAttThreshold = 0.33, tAtt2Threshold = 0.18, tDefThreshold = 0.02; //minimum required value of tAtt to send attack command
bool arduinoReady = false, attInRange = false, defInRange = false, recenterLock = false, centerHomeY = false;
bool filtPointsReqSet = false, ricochetDetected = false;
int filtPoints = 0, filtPointsReq = 4, origFiltPoints = 4;
long prevCommandTime = 0, prevState1Time = 0;
int vPxHighCounter = 0, vPxNegCounter = 0, vPxFloatingCounter = 0, retransmitCounter = 0;
float loopTime = 0, timeInState1 = 0, state1Timeout = 1; //seconds
// object tracking //
// towards: puck moving towards robot, away: puck moving away from robot, decision: latest x-value at which attack/defense must be picked
float xThreshold_towards = 50, xThreshold_away = 30, xThreshold_decision = 35;
float xPTfilt = 0, yPTfilt = 0, vPxfilt = 0, vPyfilt = 0;
float xFloatingVMin = -20, xFloatingVMax = 20, xFloatingLimit = 25;
int noPuckCounter = 0;
boolean forceRecenter = false;
int state = 0, prev_state = -1;
/*
enum StateTypes {
IDLE = 0,
PUCK_TWD_ROBOT = 1,
TRANSMIT_ATKDEF = 2,
RECENTER = 3
};
*/
while(true) {
// Copy image from camera thread locally
Mat imgOriginal;
mtx.lock();
if(!cameraUpdated) { mtx.unlock(); continue; } //go to next loop iteration if camera hasn't updated the image
imgCapture.copyTo(imgOriginal);
cameraUpdated = false;
mtx.unlock();
static int tick = 0;
tick++;
if (writeFrameInfo) {
debugInfo << "**************" << endl;
debugInfo << "Frame " << tick << endl;
debugInfo << "Time: " << (double)getTickCount()/getTickFrequency() << endl;
}
// check com port for updates
asInterface.readComPort(state);
//extract puck position from image, maintain state variables
int trackStatus = puckTracker.UpdatePuckState(imgOriginal);
//int robotStatus = robotTracker.UpdatePuckState(imgOriginal);
bool positionUpdated = (trackStatus & 1) == 1, velocityUpdated = (trackStatus & 2) == 2;
if(!velocityUpdated) { vPx = 100; vPy = 0; }
//if(!(trackStatus & 1 == 1)) { continue; } //go to next iteration if position wasn't updated
noPuckCounter = !velocityUpdated || (xPT > xFloatingLimit && vPx > 0) ? noPuckCounter + 1 : 0;
if (noPuckCounter == 3) {
forceRecenter = true;
vPxHighCounter = 1;
vPxNegCounter = 0;
vPxFloatingCounter = 0;
}
//cout << "noPuckCounter = " << noPuckCounter << endl;
//if(state == 1) { cout << xPT << "," << yPT << "\t" << vPx << "\t" << vPy << endl; }
//cout << positionUpdated << "," << velocityUpdated << "\t" << puckTracker.xPC << "," << puckTracker.yPC;
//cout << "\t" << puckTracker.xPT << "," << puckTracker.yPT << endl;
//predict trajectory
Mat imgTrajectory = Mat::zeros( imgOriginal.size(), CV_8UC4 );
if (velocityUpdated) {
vPxHighCounter = vPx > xFloatingVMax ? vPxHighCounter + 1 : 0;
vPxNegCounter = vPx < xFloatingVMin ? vPxNegCounter + 1 : 0;
vPxFloatingCounter = vPxHighCounter || vPxNegCounter ? 0 : vPxFloatingCounter + 1;
}
//if (vPxHighCounter % 100 == 1 || vPxNegCounter %100 == 1 || vPxFloatingCounter % 100 == 1) {
// cout << "C: " << xPT << "," << yPT << "\t" << vPx << "," << vPy << "\t" << vPxHighCounter << "," << vPxNegCounter << "," << vPxFloatingCounter << endl;
//}
//cout << xPC << "," << yPC << "\t" << xPT << "," << yPT << endl;
//continue;
if (writeFrameInfo) {
debugInfo << "Position: " << xPT << "," << yPT << "\t" << vPx << "," << vPy << endl;
debugInfo << "Counters: " << vPxHighCounter << "," << vPxNegCounter << "," << vPxFloatingCounter << endl;
debugInfo << "State: " << state << endl;
}
#pragma region "State Machine"
int next_state = state;
switch(state) {
case 0: { //idle -- default state
//entry
if(prev_state != 0) { cout << "Entered state 0" << endl; }
if (noPuckCounter == 0 && vPxFloatingCounter >= 5 && xPT <= xFloatingLimit) {
float nx = xPT + vPx / 5.0 + 4;
float ny = yPT + vPy / 5.0;
if (ny < YATT_LOW) ny = YATT_LOW;
if (ny > YATT_HIGH) ny = YATT_HIGH;
if (nx > xFloatingLimit) nx = xFloatingLimit;
cout << "Pushing the puck: " << nx << "," << ny << endl;
if (writeFrameInfo) {
debugInfo << "Pushing the puck: " << nx << "," << ny << endl;
}
asInterface.sendDefendCommand(nx, ny, 0.001);
next_state = 0;
recenterLock = false;
break;
}
// state transitions -- wait for puck to come towards robot //
next_state = vPxNegCounter > 1 && xPT <= xThreshold_towards ? 1 : 0; //transition to receive puck
//next_state = xPT >= xThreshold_away && vPxHighCounter > 2 && !recenterLock ? 3 : next_state; //recenter if puck moving away
next_state = (vPxHighCounter > 0 || (vPxFloatingCounter > 0 && xPT > xFloatingLimit)) && !recenterLock ? 3 : next_state; //recenter if puck moving away
if (forceRecenter && !recenterLock) { next_state = 3; }
// exit actions
if(next_state == 1) {
//cout << "State 0: " << xPT << "," << yPT << "\t" << vPx << "\t" << vPy << endl;
puckTracker.PredictPuckTrajectory(&imgTrajectory, displayEnabled, numberOfCritXvalues, critXvalues,
critYvalues, critTvalues, critVyvalues);
//cout << "State 0: " << xPT << "," << yPT << "\t" << vPx << "\t" << vPy << endl;
}
break;
}
case 1: { //puck moves player -> robot: decide between attack or defense
// entry -- clear filter values
if(prev_state != 1) {
cout << "Entered state 1" << endl;
cout << xPT << "," << yPT << "," << vPx << "," << vPy << "\t";
cout << puckTracker.prev_xPT << "," << puckTracker.prev_yPT << endl;
xPTfilt = 0; yPTfilt = 0; vPxfilt = 0; vPyfilt = 0; filtPoints = 0;
xDef = 0; yDef = 0; tDef = 0; xAtt = 0; yAtt = 0; tAtt = 0; xAtt2 = 0; yAtt2 = 0; tAtt2 = 0;
yDefArr.clear(); tDefArr.clear(); yAttArr.clear(); tAttArr.clear(); yAtt2Arr.clear(); tAtt2Arr.clear();
recenterLock = false; timeInState1 = 0;
filtPointsReq = 4;
}
bool decide = false, noMoves = false;
if(velocityUpdated) { //only proceed if we got a position/velocity update
// adjust required number of filter points based on x-velocity
if(vPx < -350) { filtPointsReq = min(filtPointsReq, 2); }
else if(vPx < -200) { filtPointsReq = min(filtPointsReq, 3); }
else { filtPointsReq = min(filtPointsReq, 4); }
// state actions -- predict puck trajectory, decide on attack/defense when conditions met
puckTracker.PredictPuckTrajectory(&imgTrajectory, displayEnabled, numberOfCritXvalues, critXvalues,
critYvalues, critTvalues, critVyvalues);
// clear filter and only require 2 points after a ricochet
if(puckTracker.yRicochetOccurred) {
if (writeFrameInfo) {
debugInfo << "Ricochet Occurred" << endl;
}
xDef = 0; yDef = 0; tDef = 0; xAtt = 0; yAtt = 0; tAtt = 0;xAtt2 = 0; yAtt2 = 0; tAtt2 = 0;
yDefArr.clear(); tDefArr.clear(); yAttArr.clear(); tAttArr.clear(); yAtt2Arr.clear(); tAtt2Arr.clear();
filtPointsReq = 2; filtPoints = 0;
puckTracker.yRicochetOccurred = false; //clear flag
timeInState1 += loopTime;
next_state = 1;
break;
}
filtPoints++;
cout << "Curr: " << xPT << "," << yPT << "\t" << vPx << "," << vPy << endl;
//cout << "Crit1: " << critXvalues[1] << "," << critYvalues[1] << "," << critTvalues[1] << endl;
//cout << "Crit4: " << critXvalues[4] << "," << critYvalues[4] << "," << critTvalues[4] << endl;
// check time at critXvalues[1] -- attack point
xAtt = critXvalues[1]; yAtt += critYvalues[1]; tAtt += critTvalues[1];
yAttArr.push_back(critYvalues[1]); tAttArr.push_back(critTvalues[1]);
xAtt2 = critXvalues[2]; yAtt2 += critYvalues[2]; tAtt2 += critTvalues[2];
yAtt2Arr.push_back(critYvalues[2]); tAtt2Arr.push_back(critTvalues[2]);
// check y, t values before adding to filter
//yAtt += checkBounds(critYvalues[1], yAtt, YATT_HIGH, YATT_LOW, filtPoints);
//tAtt += checkBounds(critTvalues[1], tAtt, 0, 100, filtPoints);
// determine command point for defense
int idx = numberOfCritXvalues - 1;
xDef = critXvalues[idx]; yDef += critYvalues[idx]; tDef += critTvalues[idx];
yDefArr.push_back(critYvalues[idx]); tDefArr.push_back(critTvalues[idx]);
if (filtPoints > 5) {
tAtt -= tAttArr.front(); tAttArr.pop_front();
yAtt -= yAttArr.front(); yAttArr.pop_front();
tAtt2 -= tAtt2Arr.front(); tAtt2Arr.pop_front();
yAtt2 -= yAtt2Arr.front(); yAtt2Arr.pop_front();
tDef -= tDefArr.front(); tDefArr.pop_front();
yDef -= yDefArr.front(); yDefArr.pop_front();
filtPoints--;
}
//yDef += checkBounds(critYvalues[idx], yDef, yBoundLow, yBoundHigh, filtPoints);
//tDef += checkBounds(critTvalues[idx], tDef, 0, 100, filtPoints);
// compute attack and defend points + times, randomize attack angle
decide = filtPoints >= filtPointsReq;// || (xPT <= xThreshold_decision && filtPoints > 1);
//cout << xDef << "," << yDef << "," << tDef << "\t" << xAtt << "," << yAtt << "," << tAtt << endl;
cout << xDef << "," << critYvalues[idx] << "," << critTvalues[idx] << "\t";
cout << xAtt << "," << critYvalues[1] << "," << critTvalues[1] << "\t";
cout << xAtt2 << "," << critYvalues[2] << "," << critTvalues[2] << "\t";
cout << vPx << "," << vPy << endl;
noMoves = false;
if(decide) {
//send once here for fastest response, continuously send after state 2 transition
float xD = xDef;
float yD = yDef / filtPoints;
float tD = tDef / filtPoints;
float xA = xAtt;
float yA = yAtt / filtPoints;
float tA = tAtt / filtPoints;
float xA2 = xAtt2;
float yA2 = yAtt2 / filtPoints;
float tA2 = tAtt2 / filtPoints;
//if puck is coming at an angle, offset defense point by paddle radius
//if(critVyvalues[idx]/vPx < -0.33) { yDef += rPaddle; }
//else if(critVyvalues[idx]/vPx > 0.33) { yDef -= rPaddle; }
//if(critVyvalues[idx]/vPx < -0.33) { yDef -= rPaddle; }
//else if(critVyvalues[idx]/vPx > 0.33) { yDef += rPaddle; }
// make sure commands are within bounds
if (yD < YDEF_LOW) yD = YDEF_LOW;
if (yD > YDEF_HIGH) yD = YDEF_HIGH;
bool defInRange = tD > TDEF_MIN;
bool attInRange = yA > YATT_LOW && yA < YATT_HIGH && tA > TATT_MIN;
bool att2InRange = yA2 > YATT2_LOW && yA < YATT2_HIGH && tA2 > TATT2_MIN;
if (writeFrameInfo) {
debugInfo << "A: " << xA << "," << yA << "," << tA << endl;
debugInfo << "A2: " << xA2 << "," << yA2 << "," << tA2 << endl;
debugInfo << "D: " << xD << "," << yD << "," << tD << endl;
}
if(attInRange) {
cout << "A: " << xA << "," << yA << "," << tA << endl;
if (writeFrameInfo) {
debugInfo << "Attacking: " << xA << "," << yA << "," << tA << endl;
}
asInterface.sendAttackCommand(xA,yA,tA);
}
else if(att2InRange) {
cout << "A2: " << xA2 << "," << yA2 << "," << tA2 << endl;
if (writeFrameInfo) {
debugInfo << "Attacking2: " << xA2 << "," << yA2 << "," << tA2 << endl;
}
asInterface.sendAttackCommand(xA2,yA2,tA2);
}
else if(defInRange) {
cout << "D: " << xD << "," << yD << "," << tD << endl;
if (writeFrameInfo) {
debugInfo << "Defensing: " << xD << "," << yD << "," << tD << endl;
}
asInterface.sendDefendCommand(xD,yD,tD);
}
else {
noMoves = true;
if (writeFrameInfo) {
debugInfo << "Give up!"<< endl;
debugInfo << "Defense info: " << xD << "," << yD << "," << tD << endl;
}
} //both out of range, give up
prevCommandTime = getTickCount();
}
}
timeInState1 += loopTime;
//cout << "Time: " << timeInState1 << endl;
// state transitions
next_state = timeInState1 > state1Timeout ? 0 : 1; //timeout after 1 second to avoid getting stuck waiting for more points
//next_state = decide ? 2 : next_state; //retransmit attack or defense command
next_state = decide && vPxfilt > 1 || noMoves ? 0 : next_state;
//next_state = xPT >= xThreshold_away && vPx > 1 ? 3 : next_state;
next_state = vPxHighCounter > 0 && !recenterLock ? 3 : next_state;
if(next_state != 1) { filtPointsReqSet = false; }
break;
}
case 2: {
break;
}
case 3: { //puck moves robot -> player: recenter if time is available
if(prev_state != 3) {
cout << endl << "Entered state 3" << endl << endl;
asInterface.recenterCmdAcked = false;
centerHomeY = /*forceRecenter || */ (vPx > 1 && vPx < 175); //decide whether shot is slow enough to centerhome y-axis
retransmitCounter = 3;
}
if(recenterLock && !forceRecenter) { next_state = 0; break; }
if(!asInterface.recenterCmdAcked) {
cout << "Retransmitting..." << endl;
if (writeFrameInfo) {
debugInfo << "Recentering" << endl;
}
asInterface.sendRecenterCommand(centerHomeY);
}
next_state = asInterface.recenterCmdAcked ? 0 : 3;
if(next_state != 3) { recenterLock = true; forceRecenter = false; }
break;
}
case 4: { //goal scored
//testing
//predictPuckTrajectory(&imgTrajectory, critYvalues, critTvalues);
break;
}
default: {
break;
}
}
prev_state = state;
state = next_state;
#pragma endregion
//cout << state << "\t" << xPT << "," << yPT << "\t" << xPC << "," << yPC << endl;
if(loggingEnabled) { myfile << xPT << "," << yPT << "\t" << vPx << "\t" << globalArea << "," << globalRoundness << endl; }
//bump output to display window, check for key press
//cvWaitKey() waits for ~10 ms even if a value less than 10 is given -- blame Windows
if(displayEnabled) {
// draw puck tracking lines
//if(initCounts > initCountsNeeded && prev_xPC >= 0 && prev_yPC>=0 && xPC>=0 && yPC>=0) {
/*
if(colorCycler == 0) {
line(imgLines, Point(xPC, yPC), Point(prev_xPC, prev_yPC), Scalar(0,0,255), 2);
}
else if(colorCycler == 1) {
line(imgLines, Point(xPC, yPC), Point(prev_xPC, prev_yPC), Scalar(255,0,0), 2);
}
else if(colorCycler == 2) {
line(imgLines, Point(xPC, yPC), Point(prev_xPC, prev_yPC), Scalar(0,255,0), 2);
}
colorCycler++; if(colorCycler > 2) { colorCycler = 0; }
*/
mtx.try_lock();
line(imgLines, Point(xPC, yPC), Point(puckTracker.prev_xPC, puckTracker.prev_yPC), Scalar(0,0,255), 2);
//line(imgLines, Point(robotTracker.xPC, robotTracker.yPC), Point(robotTracker.prev_xPC, robotTracker.prev_yPC), Scalar(0,255,0), 2);
mtx.unlock();
//cout << "lines added" << endl;
//}
imgOriginal = imgOriginal + imgLines + imgTrajectory;
if (writeVideo) {
if (tick == 1) {
boost::filesystem::path dir("recording");
boost::filesystem::remove_all(dir);
boost::filesystem::create_directory(dir);
}
char fileName[255];
sprintf(fileName, "recording/frame_%.4d.jpg", tick);
imwrite(string(fileName), imgOriginal );
}
imshow(OUTPUT_WINDOW, imgOriginal); //show the original image
//imshow("Contours",imgThresholded);
int keyPress = cvWaitKey(1);
//cout << "Pressed: " << keyPress << endl;
if (keyPress == 27) { //wait for 'esc' key press for 30ms. If 'esc' key is pressed, break loop
cout << "esc key is pressed by user" << endl;
break;
}
else if(keyPress == 32) { //empty out lines
imgLines = Mat::zeros( imgLines.size(), CV_8UC4 );
}
}
if (writeFrameInfo) {
debugInfo << "End time: " << (double)getTickCount()/getTickFrequency() << endl;
}
frameTimestamp = getTickCount();
loopTime = ((float)(frameTimestamp - oldFrameTimestamp))/getTickFrequency();
//cout << "Frame interval: " << t << endl;
oldFrameTimestamp = getTickCount();
}
//myfile.close();
cam -> Stop();
cvDestroyWindow(OUTPUT_WINDOW);
return 0;
}
int main (int argc, char** argv)
{
if(pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h"))
return print_help();
// Algorithm parameters:
std::string svm_filename = "../../people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml";
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 (new PointCloudT);
bool new_cloud_available_flag = false;
pcl::Grabber* interface = new pcl::OpenNIGrabber();
boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
boost::bind (&cloud_cb_, _1, cloud, &new_cloud_available_flag);
interface->registerCallback (f);
interface->start ();
// Wait for the first frame:
while(!new_cloud_available_flag)
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
new_cloud_available_flag = false;
cloud_mutex.lock (); // for not overwriting the point cloud
// 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 ();
// Main loop:
while (!viewer.wasStopped())
{
if (new_cloud_available_flag && cloud_mutex.try_lock ()) // if a new cloud is available
{
new_cloud_available_flag = false;
// Perform people detection on the new cloud:
std::vector<pcl::people::PersonCluster<PointT> > clusters; // vector containing persons clusters
people_detector.setInputCloud(cloud);
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;
for(std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
{
if(it->getPersonConfidence() > min_confidence) // draw only people with confidence above a threshold
{
// draw theoretical person bounding box in the PCL viewer:
it->drawTBoundingBox(viewer, k);
k++;
}
}
std::cout << k << " people found" << std::endl;
viewer.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 ();
}
}
return 0;
}
bool service_callback(pcl_perception::PeopleDetectionSrv::Request &req,
pcl_perception::PeopleDetectionSrv::Response &res)
{
//the node cannot be asked to detect people while someone has asked it to detect people
if (node_state != IDLE) {
ROS_WARN("[segbot_pcl_person_detector] Person detection service may not be called until previous call is processed");
return true;
}
node_state = DETECTING;
ROS_INFO("[general_perception.cpp] Received command %s",req.command.c_str());
//command for detecting people standing up
if (req.command == "reocrd_cloud") {
PointCloudT::Ptr aggregated_cloud (new PointCloudT);
bool collecting_clouds = true;
int num_saved = 0;
while (collecting_clouds) {
//collect to grab cloud
if (new_cloud_available_flag && cloud_mutex.try_lock ()) // if a new cloud is available
{
ROS_INFO("Adding next cloud...");
*aggregated_cloud += *cloud;
num_saved++;
if (num_saved > num_frames_for_detection) {
collecting_clouds = false;
}
//unlock mutex and reset flag
new_cloud_available_flag = false;
}
//ROS_INFO("unlocking mutex...");
cloud_mutex.unlock ();
ros::spinOnce();
}
if (useVoxelGridFilter) {
pcl::PCLPointCloud2::Ptr pcl_pc (new pcl::PCLPointCloud2) ;
pcl::toPCLPointCloud2( *aggregated_cloud,*pcl_pc);
//filter each step
pcl::VoxelGrid< pcl::PCLPointCloud2 > sor;
sor.setInputCloud (pcl_pc);
sor.setLeafSize (0.005, 0.005, 0.005);
sor.filter (*pcl_pc);
//covert back
pcl::fromPCLPointCloud2(*pcl_pc, *aggregated_cloud);
}
if (useStatOutlierFilter) {
// Create the filtering object
pcl::StatisticalOutlierRemoval<PointT> sor;
sor.setInputCloud (aggregated_cloud);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*aggregated_cloud);
}
visualizeCloud(aggregated_cloud);
/*current_frame_counter = 0;
bool collecting_cloud = true;
node_state = DETECTING;
while (collecting_cloud){
if (new_cloud_available_flag && cloud_mutex.try_lock ()) // if a new cloud is available
{
ROS_INFO("Detecting on frame %i",current_frame_counter);
new_cloud_available_flag = false;
// Draw cloud and people bounding boxes in the viewer:
//increment counter and see if we need to stop detecting
current_frame_counter++;
if (current_frame_counter > num_frames_for_detection){
collecting_cloud = false;
}
}
cloud_mutex.unlock ();
//spin to collect next point cloud
ros::spinOnce();
if (visualize){
visualizeCloud();
}
}*/
}
node_state = IDLE;
return true;
}
