inline void
cvToCloud(const cv::Mat_<cv::Point3f>& points3d, pcl::PointCloud<PointT>& cloud, const cv::Mat& mask = cv::Mat())
{
cloud.clear();
cloud.width = points3d.size().width;
cloud.height = points3d.size().height;
cv::Mat_<cv::Point3f>::const_iterator point_it = points3d.begin(), point_end = points3d.end();
const bool has_mask = !mask.empty();
cv::Mat_<uchar>::const_iterator mask_it;
if (has_mask)
mask_it = mask.begin<uchar>();
for (; point_it != point_end; ++point_it, (has_mask ? ++mask_it : mask_it))
{
if (has_mask && !*mask_it)
continue;
cv::Point3f p = *point_it;
if (p.x != p.x && p.y != p.y && p.z != p.z) //throw out NANs
continue;
PointT cp;
cp.x = p.x;
cp.y = p.y;
cp.z = p.z;
cloud.push_back(cp);
}
}
void
load_pointcloud_from_file(char *filename, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud)
{
int r, g, b;
long int num_points;
pcl::PointXYZRGB p3D;
pointcloud->clear();
FILE *f = fopen(filename, "r");
if (f == NULL)
{
carmen_warn("Error: The pointcloud '%s' not exists!\n", filename);
return;
}
fscanf(f, "%ld\n", &num_points);
for (long i = 0; i < num_points; i++)
{
fscanf(f, "%f %f %f %d %d %d\n",
&p3D.x, &p3D.y, &p3D.z,
&r, &g, &b
);
p3D.r = (unsigned char) r;
p3D.g = (unsigned char) g;
p3D.b = (unsigned char) b;
pointcloud->push_back(p3D);
}
fclose(f);
}
void depthPointsHandler(const sensor_msgs::PointCloud2ConstPtr& depthPoints2)
{
frameCount = (frameCount + 1) % 4;
if (frameCount != 0) {
return;
}
pcl::PointCloud<DepthPoint>::Ptr depthPointsCur = depthPoints[0];
depthPointsCur->clear();
pcl::fromROSMsg(*depthPoints2, *depthPointsCur);
for (int i = 0; i < keyframeNum - 1; i++) {
depthPoints[i] = depthPoints[i + 1];
depthPointsTime[i] = depthPointsTime[i + 1];
}
depthPoints[keyframeNum - 1] = depthPointsCur;
depthPointsTime[keyframeNum - 1] = depthPoints2->header.stamp.toSec();
keyframeCount++;
if (keyframeCount >= keyframeNum && depthPointsTime[0] >= lastPubTime) {
depthPointsStacked->clear();
for (int i = 0; i < keyframeNum; i++) {
*depthPointsStacked += *depthPoints[i];
}
sensor_msgs::PointCloud2 depthPoints3;
pcl::toROSMsg(*depthPointsStacked, depthPoints3);
depthPoints3.header.frame_id = "camera";
depthPoints3.header.stamp = depthPoints2->header.stamp;
depthPointsPubPointer->publish(depthPoints3);
lastPubTime = depthPointsTime[keyframeNum - 1];
}
}
/**
* \breif convert an opencv collection of points to a pcl::PoinCloud, your opencv mat should have NAN's for invalid points.
* @param points3d opencv matrix of nx1 3 channel points
* @param cloud output cloud
* @param rgb the rgb, required, will color points
* @param mask the mask, required, must be same size as rgb
*/
inline void
cvToCloudXYZRGB(const cv::Mat_<cv::Point3f>& points3d, pcl::PointCloud<pcl::PointXYZRGB>& cloud, const cv::Mat& rgb,
const cv::Mat& mask, bool brg = true)
{
cloud.clear();
cv::Mat_<cv::Point3f>::const_iterator point_it = points3d.begin(), point_end = points3d.end();
cv::Mat_<cv::Vec3b>::const_iterator rgb_it = rgb.begin<cv::Vec3b>();
cv::Mat_<uchar>::const_iterator mask_it;
if(!mask.empty())
mask_it = mask.begin<uchar>();
for (; point_it != point_end; ++point_it, ++rgb_it)
{
if(!mask.empty())
{
++mask_it;
if (!*mask_it)
continue;
}
cv::Point3f p = *point_it;
if (p.x != p.x && p.y != p.y && p.z != p.z) //throw out NANs
continue;
pcl::PointXYZRGB cp;
cp.x = p.x;
cp.y = p.y;
cp.z = p.z;
cp.r = (*rgb_it)[2]; //expecting in BGR format.
cp.g = (*rgb_it)[1];
cp.b = (*rgb_it)[0];
cloud.push_back(cp);
}
}
void
convertFreenectFrameToPointCloud(
libfreenect2::Registration* registration,
libfreenect2::Frame* undistorted,
libfreenect2::Frame* registered,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud)
{
cloud->clear();
float x, y, z, rgb;
for (int c = 0; c < 512; c++)
{
for (int r = 0; r < 424; r++)
{
registration->getPointXYZRGB(undistorted, registered, r, c, x, y, z, rgb);
if (std::isnan(x)) continue;
pcl::PointXYZRGB point;
point.x = x;
point.y = y;
point.z = z;
point.rgb = rgb;
cloud->points.push_back (point);
}
}
}
/**
* \breif convert an opencv collection of points to a pcl::PoinCloud, your opencv mat should have NAN's for invalid points.
* @param points3d opencv matrix of nx1 3 channel points
* @param cloud output cloud
* @param rgb the rgb, required, will color points
* @param mask the mask, required, must be same size as rgb
*/
inline void
cvToCloudXYZRGBNormal(const cv::Mat& cvcloud, pcl::PointCloud<pcl::PointXYZRGBNormal>& cloud)
{
std::size_t rows = cvcloud.rows;
std::size_t cols = cvcloud.cols;
cloud.clear();
for (std::size_t i = 0; i<rows; ++i) {
pcl::PointXYZRGBNormal cp;
cp.x = cvcloud.at<float>(i,0);
cp.y = cvcloud.at<float>(i,1);
cp.z = cvcloud.at<float>(i,2);
if (cols > 3)
{
cp.normal_x = cvcloud.at<float>(i,3);
cp.normal_y = cvcloud.at<float>(i,4);
cp.normal_z = cvcloud.at<float>(i,5);
}
if (cols > 6)
{
cp.r = cvcloud.at<float>(i,6);
cp.g = cvcloud.at<float>(i,7);
cp.b = cvcloud.at<float>(i,8);
}
cloud.push_back(cp);
}
}
void
Triangulation::convertSurface2Vertices (const ON_NurbsSurface &nurbs, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
std::vector<pcl::Vertices> &vertices, unsigned resolution)
{
// copy knots
if (nurbs.KnotCount (0) <= 1 || nurbs.KnotCount (1) <= 1)
{
printf ("[Triangulation::convertSurface2Vertices] Warning: ON knot vector empty.\n");
return;
}
cloud->clear ();
vertices.clear ();
double x0 = nurbs.Knot (0, 0);
double x1 = nurbs.Knot (0, nurbs.KnotCount (0) - 1);
double w = x1 - x0;
double y0 = nurbs.Knot (1, 0);
double y1 = nurbs.Knot (1, nurbs.KnotCount (1) - 1);
double h = y1 - y0;
createVertices (cloud, float (x0), float (y0), 0.0f, float (w), float (h), resolution, resolution);
createIndices (vertices, 0, resolution, resolution);
for (auto &v : *cloud)
{
double point[9];
nurbs.Evaluate (v.x, v.y, 1, 3, point);
v.x = static_cast<float> (point[0]);
v.y = static_cast<float> (point[1]);
v.z = static_cast<float> (point[2]);
}
}
template <typename VoxelT, typename WeightT> void
pcl::TSDFVolume<VoxelT, WeightT>::convertToTsdfCloud (pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) const
{
int sx = header_.resolution(0);
int sy = header_.resolution(1);
int sz = header_.resolution(2);
const int step = 2;
const int cloud_size = header_.getVolumeSize() / (step*step*step);
cloud->clear();
cloud->reserve (std::min (cloud_size/10, 500000));
int volume_idx = 0, cloud_idx = 0;
//#pragma omp parallel for // if used, increment over idx not possible! use index calculation
for (int z = 0; z < sz; z+=step)
for (int y = 0; y < sy; y+=step)
for (int x = 0; x < sx; x+=step, ++cloud_idx)
{
volume_idx = sx*sy*z + sx*y + x;
// pcl::PointXYZI &point = cloud->points[cloud_idx];
if (weights_->at(volume_idx) == 0 || volume_->at(volume_idx) > 0.98 )
continue;
pcl::PointXYZI point;
point.x = x; point.y = y; point.z = z;//*64;
point.intensity = volume_->at(volume_idx);
cloud->push_back (point);
}
// cloud->width = cloud_size;
// cloud->height = 1;
}
void SNA::cloud_cb(const sensor_msgs::PointCloud2::ConstPtr& cloud)
{
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::toPCL(*cloud,pcl_pc2);
pcl::PointCloud<pcl::PointXYZ> buffer_cloud;
pcl::fromPCLPointCloud2(pcl_pc2,buffer_cloud);
pcl::PointXYZ temp;
for(size_t i=0;i<buffer_cloud.size();i++)
{
temp.x = buffer_cloud.points[i].x;
temp.y = buffer_cloud.points[i].y;
temp.z = buffer_cloud.points[i].z;
assembly_.push_back(temp);
}
pcl::toROSMsg(assembly_,output_);
if(!is_moving_)
{
assembly_.clear();
}
else
{
output_.header.frame_id = "/ChestLidar";
assembled_cloud_pub_.publish(output_);
if(dxl_err_ < 0.01){
remap_ = output_;
assembled_cloud_pub_.publish(remap_);
}
}
}
void laserCloudLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudLast2)
{
timeLaserCloudLast = laserCloudLast2->header.stamp.toSec();
laserCloudLast->clear();
pcl::fromROSMsg(*laserCloudLast2, *laserCloudLast);
newLaserCloudLast = true;
}
void SimpleCloudGrabber::copyCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &source,
pcl::PointCloud<pcl::PointXYZ>::Ptr &dest)
{
dest->clear();
for (pcl::PointXYZ pt : source->points)
{
dest->points.push_back(pt);
}
dest->width = dest->points.size();
dest->height = 1;
}
bool RegionGrowing::seedRegionGrowing(
pcl::PointCloud<PointNormalT>::Ptr src_points,
const PointT seed_point, const PointCloud::Ptr cloud,
PointNormal::Ptr normals) {
if (cloud->empty() || normals->size() != cloud->size()) {
ROS_ERROR("- Region growing failed. Incorrect inputs sizes ");
return false;
}
if (isnan(seed_point.x) || isnan(seed_point.y) || isnan(seed_point.z)) {
ROS_ERROR("- Seed Point is Nan. Skipping");
return false;
}
this->kdtree_->setInputCloud(cloud);
std::vector<int> neigbor_indices;
this->getPointNeigbour<int>(neigbor_indices, seed_point, 1);
int seed_index = neigbor_indices[0];
const int in_dim = static_cast<int>(cloud->size());
int *labels = reinterpret_cast<int*>(malloc(sizeof(int) * in_dim));
#ifdef _OPENMP
#pragma omp parallel for num_threads(this->num_threads_)
#endif
for (int i = 0; i < in_dim; i++) {
if (i == seed_index) {
labels[i] = 1;
}
labels[i] = -1;
}
this->seedCorrespondingRegion(labels, cloud, normals,
seed_index, seed_index);
src_points->clear();
for (int i = 0; i < in_dim; i++) {
if (labels[i] != -1) {
PointNormalT pt;
pt.x = cloud->points[i].x;
pt.y = cloud->points[i].y;
pt.z = cloud->points[i].z;
pt.r = cloud->points[i].r;
pt.g = cloud->points[i].g;
pt.b = cloud->points[i].b;
pt.normal_x = normals->points[i].normal_x;
pt.normal_y = normals->points[i].normal_y;
pt.normal_z = normals->points[i].normal_z;
src_points->push_back(pt);
}
}
free(labels);
return true;
}
void ldr_to_cloud(const Eigen::MatrixXf& ldr_data, pcl::PointCloud<pcl::PointXYZ>& cloud)
{
cloud.clear();
cloud.is_dense = false;
cloud.points.resize(ldr_data.rows());
for (int r=0; r < ldr_data.rows(); r++)
{
pcl::PointXYZ& p = cloud.at(r);
p.x = ldr_data(r, 0);
p.y = ldr_data(r, 1);
p.z = ldr_data(r, 2);
}
}
void computeGradient(const pcl::PointCloud<pcl::PointXY> ¤t_2d_scan, pcl::PointCloud<pcl::PointXY> &gradient_scan)
{
unsigned int n_points = current_2d_scan.size();
gradient_scan.clear();
gradient_scan.resize(n_points);
for(unsigned int i = 0; i < n_points; i++)
{
pcl::PointXY grad;
grad.x = abs( current_2d_scan[i].x - prev_2d_scan[i].x ); // dx
grad.y = abs( current_2d_scan[i].y - prev_2d_scan[i].y ); // dy
gradient_scan[i] = grad;
}
};
int
accumulate_clouds(carmen_velodyne_partial_scan_message *velodyne_message, char *velodyne_storage_dir)
{
static char cloud_name[1024];
static int first_iteraction = 1;
static carmen_pose_3D_t zero_pose;
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_pointcloud;
static rotation_matrix *r_matrix_car_to_global;
int current_point_cloud_index;
carmen_vector_3D_t robot_velocity = {0.0, 0.0, 0.0};
double pose_timestamp = 0.0;
double phi = 0.0;
if (first_iteraction)
{
memset(&zero_pose, 0, sizeof(carmen_pose_3D_t));
source_pointcloud = boost::shared_ptr< pcl::PointCloud<pcl::PointXYZRGB> >(new pcl::PointCloud<pcl::PointXYZRGB>);
r_matrix_car_to_global = compute_rotation_matrix(r_matrix_car_to_global, zero_pose.orientation);
first_iteraction = 0;
}
accumulate_partial_scan(velodyne_message, &velodyne_params, &velodyne_data);
if (two_complete_clouds_have_been_acquired())
{
current_point_cloud_index = velodyne_data.point_cloud_index;
velodyne_data.robot_pose[velodyne_data.point_cloud_index] = zero_pose;
velodyne_data.robot_phi[velodyne_data.point_cloud_index] = phi;
velodyne_data.robot_velocity[velodyne_data.point_cloud_index] = robot_velocity;
velodyne_data.robot_timestamp[velodyne_data.point_cloud_index] = pose_timestamp;
add_velodyne_spherical_points_to_pointcloud(source_pointcloud, &(velodyne_data.points[current_point_cloud_index]), velodyne_data.intensity[current_point_cloud_index], r_matrix_car_to_global,
&zero_pose, &velodyne_params, &velodyne_data);
sprintf(cloud_name, "%s/%lf.ply", velodyne_storage_dir, velodyne_message->timestamp);
save_pointcloud_to_file(cloud_name, source_pointcloud);
// DEBUG:
// char cloud_name[1024];
// sprintf(cloud_name, "%s/CLOUDS-%lf.ply", velodyne_storage_dir, velodyne_message->timestamp);
// pcl::io::savePLYFile(cloud_name, *source_pointcloud);
source_pointcloud->clear();
return 1;
}
return 0;
}
void
Triangulation::convertCurve2PointCloud (const ON_NurbsCurve &curve, const ON_NurbsSurface &surf,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, unsigned resolution)
{
// copy knots
if (curve.m_knot_capacity <= 1)
{
printf ("[Triangulation::Convert] Warning: ON knot vector empty.\n");
return;
}
cloud->clear ();
if (resolution < 2)
resolution = 2;
int cp_red = curve.Order () - 2;
// for each element of the nurbs curve
for (int i = 1; i < curve.KnotCount () - 1 - cp_red; i++)
{
double dr = 1.0 / (resolution - 1);
double xi0 = curve.m_knot[i];
double xid = (curve.m_knot[i + 1] - xi0);
for (unsigned j = 0; j < resolution; j++)
{
pcl::PointXYZRGB pt;
double xi = (xi0 + j * dr * xid);
double p[3];
double pp[3];
curve.Evaluate (xi, 0, 2, pp);
surf.Evaluate (pp[0], pp[1], 0, 3, p);
pt.x = p[0];
pt.y = p[1];
pt.z = p[2];
pt.r = 255;
pt.g = 0;
pt.b = 0;
cloud->push_back (pt);
}
}
}
// Methods
// Return a first list of points of objects which has a proper size
void findCandidates(const kut_ugv_sensor_fusion::lidar_object_listConstPtr& object_list, pcl::PointCloud<pcl::PointXY> ¤t_2d_scan)
{
current_2d_scan.clear();
for(unsigned int i = 0; i < object_list->object.size(); i++)
{
// Check the size of the object
if( object_list->object[i].width > OBJECT_MIN_WIDTH && object_list->object[i].height > OBJECT_MIN_HEIGHT
&& object_list->object[i].width < OBJECT_MAX_WIDTH && object_list->object[i].height < OBJECT_MAX_HEIGHT)
{
pcl::PointXY p;
p.x = object_list->object[i].centroid.x;
p.y = object_list->object[i].centroid.y;
current_2d_scan.push_back(p);
}
}
}
void setTrackerTarget(){
initTracking();
Eigen::Vector4f c;
Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
pcl::compute3DCentroid<PointT>(*object_to_track,c);
trans.translation().matrix() = Eigen::Vector3f(c[0],c[1],c[2]);
tracker_->setTrans (trans);
pcl::PointCloud<PointT>::Ptr tmp_pc(new pcl::PointCloud<PointT>);
pcl::transformPointCloud<PointT> (*object_to_track, *tmp_pc, trans.inverse());
tracker_->setReferenceCloud(tmp_pc);
tracker_->setInputCloud(cloud);
tracked_object_centroid->clear();
tracked_object_centroid->push_back(PointT(c[0],c[1],c[2]));
}
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud)
{
seconds++;
boost::mutex::scoped_lock updateOnlineLock(updateOnlineMutex);
onlineView->clear();
pcl::copyPointCloud(*cloud, *onlineView);
updateOnline = true;
updateOnlineLock.unlock();
boost::mutex::scoped_lock lock( updateCloudBMutex );
if(seconds % 5 == 0 && start && !stop)
{
PointCloud<pcl::PointXYZRGB>::Ptr deep_copy (new PointCloud<pcl::PointXYZRGB>( *onlineView ) );
cloudB = deep_copy;
capturedNew = true;
noFrames++;
condQ.notify_one();
}
}
void
Triangulation::convertCurve2PointCloud (const ON_NurbsCurve &nurbs, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
unsigned resolution)
{
// copy knots
if (nurbs.m_knot_capacity <= 1)
{
printf ("[Triangulation::convert] Warning: ON knot vector empty.\n");
return;
}
cloud->clear ();
if (resolution < 2)
resolution = 2;
int cp_red = nurbs.Order () - 2;
// for each element in the nurbs curve
for (int i = 1; i < nurbs.KnotCount () - 1 - cp_red; i++)
{
double dr = 1.0 / (resolution - 1);
double xi0 = nurbs.m_knot[i];
double xid = (nurbs.m_knot[i + 1] - xi0);
for (unsigned j = 0; j < resolution; j++)
{
double xi = (xi0 + j * dr * xid);
pcl::PointXYZRGB p;
double points[3];
nurbs.Evaluate (xi, 0, 3, points);
p.x = static_cast<float> (points[0]);
p.y = static_cast<float> (points[1]);
p.z = static_cast<float> (points[2]);
p.r = 255;
p.g = 0;
p.b = 0;
cloud->push_back (p);
}
}
}
/**
* @brief Fills a point cloud with all the pixels in a collection of keyframes.
*/
void utils::generateCloud(const std::vector<KeyframeContainer>& frames,pcl::PointCloud<pcl::PointXYZRGB>& newCloud)
{
newCloud.clear();
for(uint f=0;f<frames.size();++f)
{
if(frames[f].projectionMatrix.rows>0 && frames[f].projectionMatrix.cols>0)
{
//calculate the extended projection matrix
cv::Mat_<double> projective(4,4,0.0);
for(int x=0;x<3;x++) for(int y=0;y<4;y++)
projective(x,y)=frames[f].projectionMatrix(x,y);
projective(3,3)=1;
// all pixels in the current frame
for(int x=0;x<frames[f].colorImg.image.cols;x++)
for(int y=0;y<frames[f].colorImg.image.rows;y++)
{
float depth(frames[f].depthImg.image.at<float>(y,x));
//if the depth data is available and the pixel is inside the mask
if (isnan(depth)==0 && frames[f].mask(y,x)>0)
{
//calculate 3D point from the image data
cv::Vec<uint8_t,3> pixel(frames[f].colorImg.image.at<cv::Vec<uint8_t,3> >(y,x));
cv::Mat_<double> tmpPoint2D(2,1,0.0);
tmpPoint2D(0,0)=(double)x;
tmpPoint2D(1,0)=(double)y;
cv::Mat_<double> tmpPointHomo3D=utils::reprojectImagePointTo3D(tmpPoint2D,frames[f].cameraCalibration,projective,depth);
//convert to pcl format
pcl::PointXYZRGB tmpPoint;
tmpPoint.x=tmpPointHomo3D(0,0);
tmpPoint.y=tmpPointHomo3D(1,0);
tmpPoint.z=tmpPointHomo3D(2,0);
tmpPoint.b=pixel.val[0];
tmpPoint.g=pixel.val[1];
tmpPoint.r=pixel.val[2];
//add to the cloud
newCloud.push_back(tmpPoint);
}
}
}
}
}
void UpdateCloud(const vector<Point3d>& point_cloud, const int r, const int g, const int b, bool clear)
{
boost::mutex::scoped_lock updateLock(update_pc_model_mutex);
update_pc = true;
if (clear)
cloud_ptr->clear();
for (int i=0; i<point_cloud.size(); i++)
{
Vec3b rgbv(r, g, b);
// Check for invalid points:
if (point_cloud[i].x != point_cloud[i].x ||
point_cloud[i].y != point_cloud[i].y ||
point_cloud[i].z != point_cloud[i].z ||
isnan(point_cloud[i].x) ||
isnan(point_cloud[i].y) ||
isnan(point_cloud[i].z) ||
point_cloud[i].x > MAX_PC_VAL ||
point_cloud[i].y > MAX_PC_VAL ||
point_cloud[i].z > MAX_PC_VAL)
{
continue;
}
pcl::PointXYZRGB point;
point.x = point_cloud[i].x * X_SCALE;
point.y = point_cloud[i].y * Y_SCALE;
point.z = point_cloud[i].z * Z_SCALE;
uint32_t rgb = ((uint32_t)rgbv[2] << 16 | (uint32_t)rgbv[1] << 8 | (uint32_t)rgbv[0]);
point.rgb = *reinterpret_cast<float*>(&rgb);
cloud_ptr->push_back(point);
}
cloud_ptr->width = (uint32_t) cloud_ptr->points.size();
cloud_ptr->height = 1;
updateLock.unlock();
}
static void VscanCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
{
pcl::PointCloud<pcl::PointXYZ> vscan_raw;
pcl::fromROSMsg(*msg, vscan_raw);
_vscan.clear();
for (const auto &v : vscan_raw) {
if (v.x == 0 && v.y == 0)
continue;
if (v.z > _detection_height_top || v.z < _detection_height_bottom)
continue;
_vscan.push_back(v);
}
if (_vscan_flag == false) {
std::cout << "vscan subscribed" << std::endl;
_vscan_flag = true;
}
}
void keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event_,
void* viewer_void)
{
pcl::visualization::CloudViewer* viewer = static_cast<pcl::visualization::CloudViewer *> (viewer_void);
// cout << "event_.getKeySym () = " << event_.getKeySym () << " event_.keyDown () " << event_.keyDown () << endl;
if ((event_.getKeySym () == "s" || event_.getKeySym () == "S") && event_.keyDown ())
{
cout << "s clicked" << endl;
cloud->clear();
copyPointCloud(*orig_cloud,*cloud);
if (!sor_applied) {
SORFilter();
sor_applied = true;
} else {
sor_applied = false;
}
show_cloud = true;
}
if ((event_.getKeySym ().compare("1") == 0)
#ifndef WIN32
&& event_.keyDown ()
#endif
)
{
show_cloud_A = true;
show_cloud = true;
}
if ((event_.getKeySym ().compare("2") == 0)
#ifndef WIN32
&& event_.keyDown ()
#endif
)
{
show_cloud_A = false;
show_cloud = true;
}
}
template <typename PointInT> void
pcl::SurfaceReconstruction<PointInT>::reconstruct (pcl::PointCloud<PointInT> &points,
std::vector<pcl::Vertices> &polygons)
{
// Copy the header
points.header = input_->header;
if (!initCompute ())
{
points.width = points.height = 0;
points.clear ();
polygons.clear ();
return;
}
// Check if a space search locator was given
if (check_tree_)
{
if (!tree_)
{
if (input_->isOrganized ())
tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
else
tree_.reset (new pcl::search::KdTree<PointInT> (false));
}
// Send the surface dataset to the spatial locator
tree_->setInputCloud (input_, indices_);
}
// Set up the output dataset
polygons.clear ();
polygons.reserve (2 * indices_->size ()); /// NOTE: usually the number of triangles is around twice the number of vertices
// Perform the actual surface reconstruction
performReconstruction (points, polygons);
deinitCompute ();
}
void vectorToPCLPointCloud(const std::vector< Eigen::Vector3d >& pc, pcl::PointCloud< pcl::PointXYZ >& pcl_pc, double density)
{
pcl_pc.clear();
std::vector<bool> mask;
unsigned sample_count = (unsigned)(density * pc.size());
if(density <= 0.0 || pc.size() == 0)
{
return;
}
else if(sample_count >= pc.size())
{
mask.resize(pc.size(), true);
}
else
{
mask.resize(pc.size(), false);
unsigned samples_drawn = 0;
while(samples_drawn < sample_count)
{
unsigned index = rand() % pc.size();
if(mask[index] == false)
{
mask[index] = true;
samples_drawn++;
}
}
}
for(unsigned i = 0; i < pc.size(); i++)
{
if(mask[i])
pcl_pc.push_back(pcl::PointXYZ(pc[i].x(), pc[i].y(), pc[i].z()));
}
}
void track(){
//pcl::PointCloud<PointT>::Ptr cloudWithNormals = addNormalsToPointCloud(cloud_input);
tracker_->setInputCloud(cloud);
tracker_->compute();
pcl::tracking::ParticleXYZRPY result = tracker_->getResult ();
Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
//transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
Eigen::Affine3f transs = tracker_->getTrans();
pcl::transformPointCloud<PointT>(*(tracker_->getReferenceCloud ()),*tracked_object,transformation);
particles = tracker_->getParticles ();
Eigen::Vector4f c;
pcl::compute3DCentroid<PointT>(*tracked_object,c);
tracked_object_centroid->clear();
PointT pt;
pt.x = c[0]; pt.y = c[1]; pt.z = c[2];
tracked_object_centroid->push_back(pt);
retrieved_object = findNearestObject();
}
void createPC() {
if(!cloud->empty())
cloud->clear();
for( int y=0; y<480; y++) {
for( int x=0; x<640; x++) {
Scalar intensity = fgMaskMOG.at<uchar>(y,x);
if (intensity.val[0]==255) {
float d=(float)depth_image.at<ushort>(y,x);
Vec3b color = rgb_image.at<Vec3b>(y,x);
Vector4f temp;
temp << x*d,y*d,d,1.0;
Vector4f point = t_mat*temp;
pcl::PointXYZRGBA result;
result.x = point(0);
result.y = point(1);
result.z = point(2);
result.r = color.val[2];
result.g = color.val[1];
result.b = color.val[0];
cloud->points.push_back(result);
}
}
}
}
void
Triangulation::convertSurface2Vertices (const ON_NurbsSurface &nurbs, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
std::vector<pcl::Vertices> &vertices, unsigned resolution)
{
// copy knots
if (nurbs.m_knot_capacity[0] <= 1 || nurbs.m_knot_capacity[1] <= 1)
{
printf ("[Triangulation::convert] Warning: ON knot vector empty.\n");
return;
}
cloud->clear ();
vertices.clear ();
double x0 = nurbs.Knot (0, 0);
double x1 = nurbs.Knot (0, nurbs.m_knot_capacity[0] - 1);
double w = x1 - x0;
double y0 = nurbs.Knot (1, 0);
double y1 = nurbs.Knot (1, nurbs.m_knot_capacity[1] - 1);
double h = y1 - y0;
createVertices (cloud, x0, y0, 0.0, w, h, resolution, resolution);
createIndices (vertices, 0, resolution, resolution);
for (unsigned i = 0; i < cloud->size (); i++)
{
pcl::PointXYZ &v = cloud->at (i);
double point[9];
nurbs.Evaluate (v.x, v.y, 1, 3, point);
v.x = static_cast<float> (point[0]);
v.y = static_cast<float> (point[1]);
v.z = static_cast<float> (point[2]);
}
}
bool filtering::averageFilter(pcl::PointCloud<PointType>& base_cloud){
//get the cloud dimensions
if (cloud_width_ == 0 || cloud_height_ == 0){
cloud_width_ = cloud_vector_[0].width;
cloud_height_ = cloud_vector_[0].height;
cloud_size_ = cloud_width_ * cloud_height_;
}
pcl::PointCloud<pcl::PointXYZRGB> unorganized_cloud;
for (int k = 0; k < number_of_average_clouds_; ++k)
{
//iterate over points
for (int i = 0; i < cloud_width_; i++)
{
for (int j = 0; j < cloud_height_; j++)
{
pcl::PointXYZRGB point_base_cloud = base_cloud(i,j);
pcl::PointXYZRGB point_new_cloud = cloud_vector_[k + number_of_median_clouds_](i,j);
if (point_base_cloud.z != 0 && point_new_cloud.z != 0 && std::abs(point_base_cloud.z - point_new_cloud.z) < z_threshold_)
{
point_base_cloud.x = (point_base_cloud.x * (k) + point_new_cloud.x)/(k+1);
point_base_cloud.y = (point_base_cloud.y * (k) + point_new_cloud.y)/(k+1);
point_base_cloud.z = (point_base_cloud.z * (k) + point_new_cloud.z)/(k+1);
point_base_cloud.r = (point_base_cloud.r * (k) + point_new_cloud.r)/(k+1);
point_base_cloud.g = (point_base_cloud.g * (k) + point_new_cloud.g)/(k+1);
point_base_cloud.b = (point_base_cloud.b * (k) + point_new_cloud.b)/(k+1);
}
// else if (point_new_cloud.z != 0 && point_base_cloud.z == 0)
// {
// point_base_cloud.x = point_new_cloud.x;
// point_base_cloud.y = point_new_cloud.y;
// point_base_cloud.z = point_new_cloud.z;
// point_base_cloud.r = point_new_cloud.r;
// point_base_cloud.g = point_new_cloud.g;
// point_base_cloud.b = point_new_cloud.b;
// }
base_cloud(i,j) = point_base_cloud;
bool ispartofcloud = point_base_cloud.x >= xmin_ && point_base_cloud.x <= xmax_ &&
point_base_cloud.y >= ymin_ && point_base_cloud.y <= ymax_ &&
point_base_cloud.z >= zmin_ && point_base_cloud.z <= zmax_;
if (k == number_of_average_clouds_-1 && ispartofcloud)
{
unorganized_cloud.push_back(point_base_cloud);
}
}
}
}
// write unorganized cloud
base_cloud.clear();
base_cloud = unorganized_cloud;
std::cout << "Averaged with " << number_of_average_clouds_ << " clouds." << std::endl;
std::cout << "Reduced cloud by clipping to " << base_cloud.size() << " points." << std::endl;
return true;
}