template <typename PointT> void
pcl::ExtractIndices<PointT>::applyFilter (PointCloud &output)
{
if (indices_->empty () || input_->points.empty ())
{
output.width = output.height = 0;
output.points.clear ();
// If negative, copy all the data
if (negative_)
output = *input_;
return;
}
// If the set of indices equals the number of points in the input dataset
if (indices_->size () == (input_->width * input_->height) ||
indices_->size () == input_->points.size ())
{
// If negative, then return an empty cloud
if (negative_)
{
output.width = output.height = 0;
output.points.clear ();
}
// else, we need to return all points
else
output = *input_;
return;
}
// We have a set of indices which is a subset of cloud, so let's start processing
if (negative_)
{
// Prepare a vector holding all indices
std::vector<int> all_indices (input_->points.size ());
for (size_t i = 0; i < all_indices.size (); ++i)
all_indices[i] = i;
std::vector<int> indices = *indices_;
std::sort (indices.begin (), indices.end ());
// Get the diference
std::vector<int> remaining_indices;
set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
inserter (remaining_indices, remaining_indices.begin ()));
copyPointCloud (*input_, remaining_indices, output);
}
else
copyPointCloud (*input_, *indices_, output);
}
template <typename PointInT, typename PointOutT> void
pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
const pcl::PointIndices &indices,
pcl::PointCloud<PointOutT> &cloud_out)
{
copyPointCloud (cloud_in, indices.indices, cloud_out);
}
void Planar_filter::setInputCloud(PointCloudT::Ptr cloud)
{
copyPointCloud(*cloud,*_cloud);
//this->Downsample_cloud();
Downsample_cloud();
}
template <typename PointT> void
pcl::ExtractIndices<PointT>::applyFilter (PointCloud &output)
{
std::vector<int> indices;
if (keep_organized_)
{
bool temp = extract_removed_indices_;
extract_removed_indices_ = true;
applyFilterIndices (indices);
extract_removed_indices_ = temp;
output = *input_;
std::vector<pcl::PCLPointField> fields;
pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator
{
uint8_t* pt_data = reinterpret_cast<uint8_t*> (&output.points[(*removed_indices_)[rii]]);
for (int fi = 0; fi < static_cast<int> (fields.size ()); ++fi) // fi = field iterator
memcpy (pt_data + fields[fi].offset, &user_filter_value_, sizeof (float));
}
if (!pcl_isfinite (user_filter_value_))
output.is_dense = false;
}
else
{
applyFilterIndices (indices);
copyPointCloud (*input_, indices, output);
}
}
template <typename PointT> typename pcl::PointCloud<PointT>::Ptr
pcl::SupervoxelClustering<PointT>::getVoxelCentroidCloud () const
{
typename PointCloudT::Ptr centroid_copy (new PointCloudT);
copyPointCloud (*voxel_centroid_cloud_, *centroid_copy);
return centroid_copy;
}
void
RGBID_SLAM::VisualizationManager::getPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr whole_point_cloud)
{
IdToPoseMap::iterator id2pose_map_it;
IdToPointCloudMap::const_iterator id2pointcloud_map_it;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr aux_point_cloud;
aux_point_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
for (id2pose_map_it = id2pose_map_.begin();
id2pose_map_it != id2pose_map_.end();
id2pose_map_it++)
{
int kf_id = (*id2pose_map_it).first;
Eigen::Affine3d pose = (*id2pose_map_it).second;
id2pointcloud_map_it = id2pointcloud_map_.find(kf_id);
if (id2pointcloud_map_it != id2pointcloud_map_.end())
{
aux_point_cloud->clear();
copyPointCloud((*id2pointcloud_map_it).second, aux_point_cloud);
(*id2pointcloud_map_it).second->clear();
alignPointCloud(aux_point_cloud, pose.linear(), pose.translation());
*whole_point_cloud += *aux_point_cloud;
}
}
}
void laserCloudSub(const sensor_msgs::PointCloud2ConstPtr &msg)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr color_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr laser_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::toPCL(*msg, pcl_pc2);
pcl::fromPCLPointCloud2(pcl_pc2, *laser_cloud);
copyPointCloud(*laser_cloud, *color_cloud);
addCloud(color_cloud);
}
template<typename PointT> void
pcl::RandomSample<PointT>::applyFilter (PointCloud &output)
{
std::vector<int> indices;
if (keep_organized_)
{
bool temp = extract_removed_indices_;
extract_removed_indices_ = true;
applyFilter (indices);
extract_removed_indices_ = temp;
copyPointCloud (*input_, output);
// Get X, Y, Z fields
std::vector<sensor_msgs::PointField> fields;
pcl::getFields (*input_, fields);
std::vector<size_t> offsets;
for (size_t i = 0; i < fields.size (); ++i)
{
if (fields[i].name == "x" ||
fields[i].name == "y" ||
fields[i].name == "z")
offsets.push_back (fields[i].offset);
}
// For every "removed" point, set the x,y,z fields to user_filter_value_
const static float user_filter_value = user_filter_value_;
for (size_t rii = 0; rii < removed_indices_->size (); ++rii)
{
uint8_t* pt_data = reinterpret_cast<uint8_t*> (&output[(*removed_indices_)[rii]]);
for (size_t i = 0; i < offsets.size (); ++i)
{
memcpy (pt_data + offsets[i], &user_filter_value, sizeof (float));
}
if (!pcl_isfinite (user_filter_value_))
output.is_dense = false;
}
}
else
{
output.is_dense = true;
applyFilter (indices);
copyPointCloud (*input_, indices, output);
}
}
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>); // Output datasets
pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
ne.setNormalEstimationMethod( ne.AVERAGE_3D_GRADIENT);
ne.setMaxDepthChangeFactor(0.02f);
ne.setNormalSmoothingSize(10.0f);
ne.setInputCloud(cloud);
ne.compute(*cloud_normals);
copyPointCloud(*cloud, *cloud_normals);
return cloud_normals;
}
void PassThrough::applyFilter (pcl::PointCloud<PointXYZSIFT>::Ptr input, pcl::PointCloud<PointXYZSIFT> &output, std::string filter_field_name, float min, float max, bool negative)
{
CLOG(LTRACE) << "applyFilter() " << filter_field_name;
output.header = input->header;
output.sensor_origin_ = input->sensor_origin_;
output.sensor_orientation_ = input->sensor_orientation_;
std::vector<int> indices;
output.is_dense = true;
applyFilterIndices (indices, input, filter_field_name, min, max, negative);
CLOG(LTRACE)<< "Number of indices: "<< indices.size() <<endl;
copyPointCloud (*input, indices, output);
}
void
UniformSamplingWrapper::compute(PointCloudInPtr input, PointCloudOut &output)
{
float keypoint_separation = 0.01f;
IndicesPtr indices (new std::vector<int>());
PointCloud<int> leaves;
UniformSampling<PointNormal> us;
us.setRadiusSearch(keypoint_separation);
us.setInputCloud(input);
us.compute(leaves);
// Copy point cloud and return
indices->assign(leaves.points.begin(), leaves.points.end()); // can't use operator=, probably because of different allocators
copyPointCloud(*input, *indices, output);
}
template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
pcl::SupervoxelClustering<PointT>::getLabeledVoxelCloud () const
{
pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_voxel_cloud (new pcl::PointCloud<pcl::PointXYZL>);
for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
{
typename PointCloudT::Ptr voxels;
sv_itr->getVoxels (voxels);
pcl::PointCloud<pcl::PointXYZL> xyzl_copy;
copyPointCloud (*voxels, xyzl_copy);
pcl::PointCloud<pcl::PointXYZL>::iterator xyzl_copy_itr = xyzl_copy.begin ();
for ( ; xyzl_copy_itr != xyzl_copy.end (); ++xyzl_copy_itr)
xyzl_copy_itr->label = sv_itr->getLabel ();
*labeled_voxel_cloud += xyzl_copy;
}
return labeled_voxel_cloud;
}
template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
pcl::SupervoxelClustering<PointT>::getColoredVoxelCloud () const
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud = boost::make_shared< pcl::PointCloud<pcl::PointXYZRGBA> > ();
for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
{
typename PointCloudT::Ptr voxels;
sv_itr->getVoxels (voxels);
pcl::PointCloud<pcl::PointXYZRGBA> rgb_copy;
copyPointCloud (*voxels, rgb_copy);
pcl::PointCloud<pcl::PointXYZRGBA>::iterator rgb_copy_itr = rgb_copy.begin ();
for ( ; rgb_copy_itr != rgb_copy.end (); ++rgb_copy_itr)
rgb_copy_itr->rgba = label_colors_ [sv_itr->getLabel ()];
*colored_cloud += rgb_copy;
}
return colored_cloud;
}
int main (int argc, char** argv)
{
PointCloudType::Ptr cloud(new PointCloudType);
PointCloudType::PointType p0(1,2,3);
cloud->push_back(p0);
// Call without normals
{
ComputeFeatures(cloud);
}
// Call with normals
{
PointCloudWithNormalsType::Ptr cloudWithNormals(new PointCloudWithNormalsType);
copyPointCloud(*cloud, *cloudWithNormals);
ComputeFeatures(cloudWithNormals);
}
return 0;
}
void ComputeFeatures(PointCloudType::Ptr cloud)
{
// Compute the normals
pcl::NormalEstimation<PointCloudType::PointType, PointCloudWithNormalsType::PointType> normalEstimation;
normalEstimation.setInputCloud (cloud);
typedef pcl::search::KdTree<PointCloudType::PointType> TreeType;
TreeType::Ptr tree (new TreeType);
normalEstimation.setSearchMethod (tree);
PointCloudWithNormalsType::Ptr cloudWithNormals (new PointCloudWithNormalsType);
normalEstimation.setRadiusSearch (0.03);
normalEstimation.compute (*cloudWithNormals);
copyPointCloud(*cloud, *cloudWithNormals);
std::cout << cloudWithNormals->points[0].x << std::endl;
ComputeFeatures(cloudWithNormals);
}
/**
* Implements the Median Filter.
* Gets the largest value one dexel is allowed to move as argument (floating point), and the window size (integer).
* Returns a pointer to the filtered cloud.
* Relies on the PCL 1.7 implementation. [not exists in 1.6]
*/
PointCloud<pointType>::Ptr FilterHandler::medianFilter(float maxAllowedMovement, int windowSize)
{
PointCloud<pointType>::Ptr original = _cloud->makeShared(); // getting the cloud from pointer
PointCloud<pointType>::Ptr filtered (new PointCloud<pointType>); // will hold the filtered cloud
// Copy everything from the original cloud to a new cloud (will be the filtered)
copyPointCloud (*original, *filtered);
for (int y = 0; y < filtered->height; ++y)
for (int x = 0; x < filtered->width; ++x)
if (pcl::isFinite ((*original)(x, y)))
{
std::vector<float> vals;
vals.reserve (windowSize * windowSize);
// Fill in the vector of values with the depths around the interest point
for (int y_dev = -windowSize/2; y_dev <= windowSize/2; ++y_dev)
for (int x_dev = -windowSize/2; x_dev <= windowSize/2; ++x_dev)
{
if (x + x_dev >= 0 && x + x_dev < filtered->width &&
y + y_dev >= 0 && y + y_dev < filtered->height &&
pcl::isFinite ((*original)(x+x_dev, y+y_dev)))
vals.push_back ((*original)(x+x_dev, y+y_dev).z);
}
if (vals.size () == 0)
continue;
// The output depth will be the median of all the depths in the window
partial_sort (vals.begin (), vals.begin () + vals.size () / 2 + 1, vals.end ());
float new_depth = vals[vals.size () / 2];
// Do not allow points to move more than the set max_allowed_movement_
if (fabs (new_depth - (*original)(x, y).z) < maxAllowedMovement)
filtered->operator ()(x, y).z = new_depth;
else
filtered->operator ()(x, y).z = (*original)(x, y).z +
maxAllowedMovement * (new_depth - (*original)(x, y).z) / fabsf (new_depth - (*original)(x, y).z);
}
io::savePCDFile(_output, *filtered, true);
return filtered;
}
template <typename PointT> void
pcl::RadiusOutlierRemoval<PointT>::applyFilter (PointCloud &output)
{
std::vector<int> indices;
if (keep_organized_)
{
bool temp = extract_removed_indices_;
extract_removed_indices_ = true;
applyFilterIndices (indices);
extract_removed_indices_ = temp;
output = *input_;
for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator
output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
if (!pcl_isfinite (user_filter_value_))
output.is_dense = false;
}
else
{
applyFilterIndices (indices);
copyPointCloud (*input_, indices, output);
}
}
template <typename PointT> void
pcl::getPointCloudDifference (
const pcl::PointCloud<PointT> &src,
double threshold,
const typename pcl::search::Search<PointT>::Ptr &tree,
pcl::PointCloud<PointT> &output)
{
// We're interested in a single nearest neighbor only
std::vector<int> nn_indices (1);
std::vector<float> nn_distances (1);
// The input cloud indices that do not have a neighbor in the target cloud
std::vector<int> src_indices;
// Iterate through the source data set
for (int i = 0; i < static_cast<int> (src.points.size ()); ++i)
{
// Ignore invalid points in the inpout cloud
if (!isFinite (src.points[i]))
continue;
// Search for the closest point in the target data set (number of neighbors to find = 1)
if (!tree->nearestKSearch (src.points[i], 1, nn_indices, nn_distances))
{
PCL_WARN ("No neighbor found for point %lu (%f %f %f)!\n", i, src.points[i].x, src.points[i].y, src.points[i].z);
continue;
}
// Add points without a corresponding point in the target cloud to the output cloud
if (nn_distances[0] > threshold)
src_indices.push_back (i);
}
// Copy all the data fields from the input cloud to the output one
copyPointCloud (src, src_indices, output);
// Output is always dense, as invalid points in the input cloud are ignored
output.is_dense = true;
}
void
RGBID_SLAM::VisualizationManager::refresh()
{
if (visodo_ptr_->camera_pose_has_changed_ == true)
{
new_pose_ = visodo_ptr_->getSharedCameraPose();
redraw_camera_ = true;
}
if (visodo_ptr_->scene_view_has_changed_ == true)
{
boost::mutex::scoped_lock lock(visodo_ptr_->mutex_scene_view_);
scene_view_ = visodo_ptr_->scene_view_;
intensity_view_ = visodo_ptr_->intensity_view_;
depthinv_view_ = visodo_ptr_->depthinv_view_;
redraw_scene_view_ = true;
visodo_ptr_->scene_view_has_changed_ = false;
}
if (keyframe_manager_ptr_->trajectory_has_changed_ == true)
{
boost::mutex::scoped_lock lock(keyframe_manager_ptr_->mutex_odometry_);
copyTrajectory(keyframe_manager_ptr_->poses_);
keyframe_manager_ptr_->trajectory_has_changed_ = false;
//redraw_trajectory_ = true;
}
if (keyframe_manager_ptr_->new_keyframe_has_changed_ == true)
{
boost::mutex::scoped_lock lock(keyframe_manager_ptr_->mutex_new_keyframe_);
kf_id_ = keyframe_manager_ptr_->new_kf_id_;
last_KF_pose_ = keyframe_manager_ptr_->new_keyframe_pose_;
std::pair<IdToPoseMap::iterator,bool> is_new_id;
is_new_id = id2pose_map_.insert(std::make_pair(kf_id_, last_KF_pose_));
//segmentation_view_ = keyframe_manager_ptr_->rgb_image_with_keypoints_;
memcpy ((PixelRGB*)&(segmentation_view_[0]), &(keyframe_manager_ptr_->rgb_image_with_keypoints_.data[0]),
keyframe_manager_ptr_->rgb_image_with_keypoints_.cols * keyframe_manager_ptr_->rgb_image_with_keypoints_.rows*sizeof(PixelRGB));
negentropy_view_ = keyframe_manager_ptr_->negentropy_view_;
keyframe_manager_ptr_->new_keyframe_has_changed_ = false;
redraw_keyframe_ = true;
}
if (keyframe_manager_ptr_->new_pointcloud_has_changed_ == true)
{
boost::mutex::scoped_lock lock(keyframe_manager_ptr_->mutex_new_pointcloud_);
new_pc_id_ = keyframe_manager_ptr_->new_pointcloud_id_;
std::pair<IdToPointCloudMap::iterator,bool> is_new_id;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr aux_ptr;
aux_ptr.reset(new pcl::PointCloud<pcl::PointXYZRGB>) ;
copyPointCloud(keyframe_manager_ptr_->new_pointcloud_, aux_ptr);
is_new_id = id2pointcloud_map_.insert(std::make_pair(new_pc_id_, aux_ptr));
keyframe_manager_ptr_->new_pointcloud_has_changed_ = false;
redraw_pointcloud_ = true;
}
if (keyframe_manager_ptr_->pose_graph_has_changed_ == true)
{
IdToPoseMap::iterator id2pose_map_it;
for (int i=0; i<keyframe_manager_ptr_->keyframes_list_.size(); i++)
{
id2pose_map_it = id2pose_map_.find(keyframe_manager_ptr_->keyframes_list_[i]->kf_id_);
if (id2pose_map_it != id2pose_map_.end())
{
id2pose_map_[keyframe_manager_ptr_->keyframes_list_[i]->kf_id_] = keyframe_manager_ptr_->keyframes_list_[i]->getPose();
}
}
keyframe_manager_ptr_->pose_graph_has_changed_ = false;
redraw_all_pointclouds_ = true;
}
if (redraw_scene_view_)
{
scene_viewer_.viewerRGB_.showRGBImage (reinterpret_cast<unsigned char*> (&scene_view_[0]), cols_, rows_);
intensity_viewer_.viewerFloat_.showFloatImage(&intensity_view_[0], cols_, rows_, intensity_viewer_.min_val_, intensity_viewer_.max_val_,true);
redraw_scene_view_ = false;
}
if (redraw_camera_)
{
camera_viewer_.updateCamera(new_pose_);
redraw_camera_ = false;
}
if (redraw_keyframe_)
{
negentropy_viewer_.viewerRGB_.showRGBImage (reinterpret_cast<unsigned char*> (&negentropy_view_[0]), cols_, rows_);
//segmentation_viewer_.viewerRGB_.showRGBImage (reinterpret_cast<unsigned char*> (&segmentation_view_[0]), cols_, rows_);
redraw_keyframe_ = false;
}
if (redraw_pointcloud_)
{
IdToPoseMap::iterator id2pose_map_it;
id2pose_map_it = id2pose_map_.find(new_pc_id_);
if (id2pose_map_it != id2pose_map_.end())
{
camera_viewer_.updatePointCloud(id2pointcloud_map_[new_pc_id_],(*id2pose_map_it).first, (*id2pose_map_it).second);
}
redraw_pointcloud_ = false;
}
if (redraw_trajectory_)
{
camera_viewer_.updateTrajectory(trajectory_);
redraw_trajectory_ = false;
}
if (redraw_all_pointclouds_)
{
IdToPoseMap::iterator id2pose_map_it;
IdToPointCloudMap::const_iterator id2pointcloud_map_it;
for (id2pose_map_it = id2pose_map_.begin();
id2pose_map_it != id2pose_map_.end();
id2pose_map_it++)
{
int kf_id = (*id2pose_map_it).first;
Eigen::Affine3d pose = (*id2pose_map_it).second;
//camera_viewer_.updateKeyframe(pose, kf_id);
id2pointcloud_map_it = id2pointcloud_map_.find(kf_id);
if (id2pointcloud_map_it != id2pointcloud_map_.end())
{
camera_viewer_.updatePointCloud((*id2pointcloud_map_it).second, kf_id, pose);
}
}
redraw_all_pointclouds_ = false;
}
camera_viewer_.spinOnce(1);
//boost::this_thread::sleep (boost::posix_time::millisec (20));
}
template <typename PointT> void
pcl::FastBilateralFilter<PointT>::applyFilter (PointCloud &output)
{
if (!input_->isOrganized ())
{
PCL_ERROR ("[pcl::FastBilateralFilter] Input cloud needs to be organized.\n");
return;
}
copyPointCloud (*input_, output);
float base_max = std::numeric_limits<float>::min (),
base_min = std::numeric_limits<float>::max ();
for (size_t x = 0; x < output.width; ++x)
for (size_t y = 0; y < output.height; ++y)
if (pcl_isfinite (output (x, y).z))
{
if (base_max < output (x, y).z)
base_max = output (x, y).z;
if (base_min > output (x, y).z)
base_min = output (x, y).z;
}
for (size_t x = 0; x < output.width; ++x)
for (size_t y = 0; y < output.height; ++y)
if (!pcl_isfinite (output (x, y).z))
output (x, y).z = base_max;
const float base_delta = base_max - base_min;
const size_t padding_xy = 2;
const size_t padding_z = 2;
const size_t small_width = static_cast<size_t> (static_cast<float> (input_->width - 1) / sigma_s_) + 1 + 2 * padding_xy;
const size_t small_height = static_cast<size_t> (static_cast<float> (input_->height - 1) / sigma_s_) + 1 + 2 * padding_xy;
const size_t small_depth = static_cast<size_t> (base_delta / sigma_r_) + 1 + 2 * padding_z;
Array3D data (small_width, small_height, small_depth);
for (size_t x = 0; x < input_->width; ++x)
{
const size_t small_x = static_cast<size_t> (static_cast<float> (x) / sigma_s_ + 0.5f) + padding_xy;
for (size_t y = 0; y < input_->height; ++y)
{
const float z = output (x,y).z - base_min;
const size_t small_y = static_cast<size_t> (static_cast<float> (y) / sigma_s_ + 0.5f) + padding_xy;
const size_t small_z = static_cast<size_t> (static_cast<float> (z) / sigma_r_ + 0.5f) + padding_z;
Eigen::Vector2f& d = data (small_x, small_y, small_z);
d[0] += output (x,y).z;
d[1] += 1.0f;
}
}
std::vector<long int> offset (3);
offset[0] = &(data (1,0,0)) - &(data (0,0,0));
offset[1] = &(data (0,1,0)) - &(data (0,0,0));
offset[2] = &(data (0,0,1)) - &(data (0,0,0));
Array3D buffer (small_width, small_height, small_depth);
for (size_t dim = 0; dim < 3; ++dim)
{
const long int off = offset[dim];
for (size_t n_iter = 0; n_iter < 2; ++n_iter)
{
std::swap (buffer, data);
for(size_t x = 1; x < small_width - 1; ++x)
for(size_t y = 1; y < small_height - 1; ++y)
{
Eigen::Vector2f* d_ptr = &(data (x,y,1));
Eigen::Vector2f* b_ptr = &(buffer (x,y,1));
for(size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr)
*d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0;
}
}
}
if (early_division_)
{
for (std::vector<Eigen::Vector2f >::iterator d = data.begin (); d != data.end (); ++d)
*d /= ((*d)[0] != 0) ? (*d)[1] : 1;
for (size_t x = 0; x < input_->width; x++)
for (size_t y = 0; y < input_->height; y++)
{
const float z = output (x,y).z - base_min;
const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
static_cast<float> (y) / sigma_s_ + padding_xy,
z / sigma_r_ + padding_z);
output(x,y).z = D[0];
}
}
else
{
for (size_t x = 0; x < input_->width; ++x)
for (size_t y = 0; y < input_->height; ++y)
{
const float z = output (x,y).z - base_min;
const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
static_cast<float> (y) / sigma_s_ + padding_xy,
z / sigma_r_ + padding_z);
output (x,y).z = D[0] / D[1];
}
}
}
/**
* Given a vector of data points and a vector of model
* points, use ICP to register the model to the data. Add registration transformation
* to transforms vector and registration transformation from
* the track's birth frame to absoluteTransforms vector.
*/
pcl::PointCloud<pcl::PointXYZRGB> Track::updatePosition(pcl::PointCloud<pcl::PointXYZRGB> dataPTS_cloud,vector<Model> modelPTS_clouds,
int modelTODataThreshold, int separateThreshold, int matchDThresh,
int ICP_ITERATIONS, double ICP_TRANSEPSILON, double ICP_EUCLIDEANDIST, double ICP_MAXFIT)
{
matchDistanceThreshold=matchDThresh;
nukeDistanceThreshold = separateThreshold;
icp_maxIter=ICP_ITERATIONS;
icp_transformationEpsilon=ICP_TRANSEPSILON;
icp_euclideanDistance=ICP_EUCLIDEANDIST;
icp_maxfitness = ICP_MAXFIT;
pcl::PointCloud<pcl::PointXYZRGB> modelToProcess_cloud;
Eigen::Matrix4f T;
T.setIdentity();
//Figure out the identity of the model for this region
if (frameIndex == birthFrameIndex)
{
isBirthFrame=true; // Try out Doing extra work aligning the objects if it is the very first frame
//Determine what model this creature should use
if(modelPTS_clouds.size()<2){// If there is only one model, just use that one
modelIndex = 0;
}
else{
modelIndex= identify(dataPTS_cloud,modelPTS_clouds);
modelRotated = modelPTS_clouds[modelIndex].rotated;
}
}
else
{
isBirthFrame=false;
}
// double birthAssociationsThreshold = modelPTS_clouds[0].cloud.size() * .01 *modelTODataThreshold; //TODO Shouldn't hardcode this to the first!
double birthAssociationsThreshold = modelPTS_clouds[modelIndex].cloud.size() * .01 *modelTODataThreshold; //TODO Shouldn't hardcode this to the first!
double healthyAssociationsThreshold = birthAssociationsThreshold*.4; //Still healthy with 40 percent of a whole model
modelToProcess_cloud = modelPTS_clouds[modelIndex].cloud;
//STRIP 3D data
/**/
PointCloud<PointXY> dataPTS_cloud2D;
copyPointCloud(dataPTS_cloud,dataPTS_cloud2D);
PointCloud<PointXY> modelPTS_cloud2D;
copyPointCloud(modelToProcess_cloud,modelPTS_cloud2D);
PointCloud<PointXYZRGB> dataPTS_cloudStripped;
copyPointCloud(dataPTS_cloud2D,dataPTS_cloudStripped);
PointCloud<PointXYZRGB> modelPTS_cloudStripped;
copyPointCloud(modelPTS_cloud2D,modelPTS_cloudStripped);
/* */
//leave open for other possible ICP methods
bool doPCL_3DICP=true;
double fitnessin;
//PCL implementation of ICP
if(doPCL_3DICP){
if (dataPTS_cloud.size() > 1) //TODO FIX the math will throw an error if there are not enough data points
{
// Find transformation from the orgin that will optimize a match between model and target
T= calcTransformPCLRGB(dataPTS_cloud, modelToProcess_cloud,&fitnessin); //Use 3D color
//Apply the Transformation
pcl::transformPointCloud(modelToProcess_cloud,modelToProcess_cloud, T);
// pcl::transformPointCloud(modelPTS_cloudStripped,modelPTS_cloudStripped, T);
}
}
else{
//Use the 2D stripped models and data and align them
T= calcTransformPCLRGB(dataPTS_cloudStripped, modelPTS_cloudStripped, &fitnessin); // Just 2D
pcl::transformPointCloud(modelToProcess_cloud,modelToProcess_cloud, T);
}
int totalpointsBeforeRemoval=dataPTS_cloud.size();
int totalRemovedPoints = 0;
/** Remove old data points associated with the model
* delete the points under where the model was placed. The amount of points destroyed in this process gives us a metric for how healthy the track is.
*
**/
pcl::PointCloud<pcl::PointXYZRGB> dataPTSreduced_cloud;
//Removeclosestdatacloudpoints strips the 3D data and nukes points in the proximitiy of where our model has lined up
pcl::copyPointCloud(removeClosestDataCloudPoints(dataPTS_cloud, modelToProcess_cloud, nukeDistanceThreshold ),dataPTSreduced_cloud);
totalRemovedPoints = totalpointsBeforeRemoval - dataPTSreduced_cloud.size();
qDebug()<<"Removed Points from Track "<<totalRemovedPoints;
/// For Debugging we can visualize the Pointcloud
/**
pcl:: PointCloud<PointXYZRGB>::Ptr dataPTS_cloud_ptr (new pcl::PointCloud<PointXYZRGB> (dataPTS_cloud));
// copyPointCloud(modelPTS_cloud,)
transformPointCloud(modelPTS_clouds[modelIndex].cloud,modelPTS_clouds[modelIndex].cloud,T);
pcl:: PointCloud<PointXYZRGB>::Ptr model_cloud_ptrTempTrans (new pcl::PointCloud<PointXYZRGB> (modelPTS_clouds[modelIndex].cloud));
pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
viewer.showCloud(dataPTS_cloud_ptr);
int sw=0;
while (!viewer.wasStopped())
{
if(sw==0){
viewer.showCloud(model_cloud_ptrTempTrans);
sw=1;
}
else{
viewer.showCloud(dataPTS_cloud_ptr);
sw=0;
}
}
/**/
//////
///Determine Health of latest part of track
////////
// Just born tracks go here: //Should get rid of this, doesn't make sense
if (frameIndex == birthFrameIndex)
{
absoluteTransforms.push_back(T);
//Check number of data points associated with this track, if it is greater than
// the birth threshold then set birth flag to true.
//If it doesn't hit this, it never gets born ever
if ( totalRemovedPoints>birthAssociationsThreshold ) //matchScore >birthAssociationsThreshold && //Need new check for birthAssociationsthresh// closestToModel.size() >= birthAssociationsThreshold)
{
isBirthable=true;
}
}
// Tracks that are older than 1 frame get determined here
else
{
//Check the number of data points associated with this track, if it is less than
//the zombie/death threshold, then copy previous transform, and add this frame
//index to zombieFrames
//Is the track unhealthy? if so make it a zombie
if ( totalRemovedPoints<healthyAssociationsThreshold )// !didConverge)// No zombies for now! eternal life! !didConverge) //closestToModel.size() < healthyAssociationsThreshold && absoluteTransforms.size() > 2)
{
absoluteTransforms.push_back(T);
zombieIndicesIntoAbsTransforms.push_back(absoluteTransforms.size()-1);
numberOfContinuousZombieFrames++;
}
else // Not zombie frame, keep using new transforms
{
// add to transforms
absoluteTransforms.push_back(T);
numberOfContinuousZombieFrames = 0;
} //not zombie frame
}// Not first frame
// increment frame counter
frameIndex++;
return dataPTSreduced_cloud;
}
/**
* Given a cloud of datapts (an in/out variable), a vector of dataPts, an index to a specific
* point in datapts and a distanceThreshold, add the list of indices into dataPts which are
* within the distanceThreshold to dirtyPts.
*
* @param dataPTSreduced_cloud a reference to a vector of Points
* @param point_cloud_for_reduction the collection of points that we want to delete some points from
* @param distanceThreshold an integer Euclidean distance
*/
pcl::PointCloud<pcl::PointXYZRGB> Track::removeClosestDataCloudPoints(pcl::PointCloud<pcl::PointXYZRGB> point_cloud_for_reduction,pcl::PointCloud<pcl::PointXYZRGB> removal_Cloud, int distanceThreshold){
//TODO, MAKE THIS PARALLEL
//NOTE: you cannot feed a KNN searcher clouds with 1 or fewer datapoints!
//KDTREE SEARCH
pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
// Neighbors within radius search
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
double point_radius = distanceThreshold;
PointCloud<pcl::PointXYZRGB> point_cloud_flattened;//Point cloud with extra Hue Dimension crushed to 0
PointCloud<pcl::PointXYZRGB> point_cloud_for_return;
point_cloud_for_return.reserve(point_cloud_for_reduction.size());
if(point_cloud_for_reduction.size()>1){
bool *marked= new bool[point_cloud_for_reduction.size()];
memset(marked,false,sizeof(bool)*point_cloud_for_reduction.size());
bool *parmarked= new bool[point_cloud_for_reduction.size()];
memset(parmarked,false,sizeof(bool)*point_cloud_for_reduction.size());
//Make a version of the original data cloud that is flattened to z=0 but with the same indice
copyPointCloud( point_cloud_for_reduction,point_cloud_flattened);
for(int q=0; q<point_cloud_flattened.size();q++){
point_cloud_flattened.at(q).z=0;
}
pcl:: PointCloud<PointXYZRGB>::Ptr point_cloud_for_reduction_ptr (new pcl::PointCloud<PointXYZRGB> (point_cloud_flattened));
// K nearest neighbor search with Radius
kdtree.setInputCloud (point_cloud_for_reduction_ptr); //Needs to have more than 1 data pt or segfault
double t = (double)getTickCount();
//The below has been parallelized,and runs about 2X faster
/**
// iterate over points in model and remove those points within a certain distance
for (unsigned int c=0; c < removal_Cloud.size(); c++)
{
if(point_cloud_for_reduction.size()<2){
break;
}
pcl::PointXYZRGB searchPoint;
searchPoint.x = removal_Cloud.points[c].x;
searchPoint.y = removal_Cloud.points[c].y;
//Need to take z as zero when using a flattened data point cloud
searchPoint.z = 0;
// qDebug() <<"Datapts before incremental remove"<< point_cloud_for_reduction.size();
if ( kdtree.radiusSearch (searchPoint, point_radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
{
for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i){
if(point_cloud_for_reduction.size()>0) ///NOTE CHANGED FROM > 1
marked[pointIdxRadiusSearch[i]]=true;
// point_cloud_for_reduction.erase(point_cloud_for_reduction.begin()+pointIdxRadiusSearch[i]);// point_cloud_for_reduction.points[ pointIdxRadiusSearch[i] ]
}
}
}
//DESTROY ALL MARKED POINTS
for(uint q=0; q< point_cloud_for_reduction.size(); q++){
if(!marked[q]){
point_cloud_for_return.push_back(point_cloud_for_reduction.at(q));
// point_cloud_for_return.at(q) = point_cloud_for_reduction.at(q);
}
}
t = ((double)getTickCount() - t)/getTickFrequency();
cout << "::: time serial remove " << t << endl;
/**/
//PARALLEL VERSION
/// Timing
t = (double)getTickCount();
cv::parallel_for_(Range(0, removal_Cloud.size()),Remove_Parallel(&kdtree, removal_Cloud,point_cloud_for_reduction, point_radius, &parmarked) );
//DESTROY ALL MARKED POINTS
for(uint q=0; q< point_cloud_for_reduction.size(); q++){
if(!parmarked[q]){
point_cloud_for_return.push_back(point_cloud_for_reduction.at(q));
// point_cloud_for_return.at(q) = point_cloud_for_reduction.at(q);
}
}
t = ((double)getTickCount() - t)/getTickFrequency();
cout << "::: time parallel remove " << t << endl;
delete[] marked;
delete[] parmarked;
}
//point_cloud_for_reduction.resize();
return point_cloud_for_return;
}
