void KinectRegistration::kinectCloudCallback( const sensor_msgs::PointCloud2ConstPtr &msg )
{
if( receive_point_cloud_ )
{
if( !first_point_cloud_ )
{
ROS_INFO("First PointCloud received");
PointCloud temp;
pcl::fromROSMsg( *msg, temp );
removeOutliers( temp, src_ );
first_point_cloud_ = true;
}
else
{
ROS_INFO("PointCloud received");
PointCloud temp, temp1;
pcl::fromROSMsg( *msg, temp );
removeOutliers( temp, tgt_ );
transformCloud(tgt_, temp, num_clouds_ * 40 );
temp1 = src_;
temp1 += temp;
downsample( temp1, temp );
pcl::toROSMsg( temp, cloud_msg_ );
registered_cloud_pub_.publish( cloud_msg_ );
src_ = temp1;
}
receive_point_cloud_ = false;
rotateRobotino();
num_clouds_++;
}
else
return;
}
void ObjectDetector::cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*input, *input_cloud);
// if theta and y_translation are not yet set, then run segmentation
if(theta == 0.0 && y_translation == 0.0){
performSegmentation(input_cloud);
}
else{
// downsample the point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud = downsampleCloud(input_cloud);
// perform transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud = transformCloud(downsampled_cloud);
// filter floor, walls and noise so only objects remain
pcl::PointCloud<pcl::PointXYZ>::Ptr object_cloud = filterCloud(transformed_cloud);
// cluster objects & publish cluster
clusterCloud(object_cloud);
}
}
void PointCloudDisplay::targetFrameChanged()
{
message_.lock();
transformCloud();
message_.unlock();
}
void MapCloudDisplay::retransform()
{
boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
for( std::map<int, CloudInfoPtr>::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it )
{
const CloudInfoPtr& cloud_info = it->second;
transformCloud(cloud_info, false);
cloud_info->cloud_->clear();
cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(), cloud_info->transformed_points_.size());
}
}
void LaserScanVisualizer::incomingScanCallback()
{
cloud_message_.lock();
if ( scan_message_.header.frame_id.empty() )
{
scan_message_.header.frame_id = target_frame_;
}
laser_projection_.projectLaser( scan_message_, cloud_message_ );
transformCloud( cloud_message_ );
cloud_message_.unlock();
}
void LaserScanVisualizer::targetFrameChanged()
{
cloud_message_.lock();
D_CloudMessage messages;
messages.swap( cloud_messages_ );
points_.clear();
point_times_.clear();
cloud_messages_.clear();
D_CloudMessage::iterator it = messages.begin();
D_CloudMessage::iterator end = messages.end();
for ( ; it != end; ++it )
{
transformCloud( *it );
}
cloud_message_.unlock();
}
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
PointCloudSource &output, const Matrix4 &guess)
{
// Point cloud containing the correspondences of each point in <input, indices>
PointCloudSourcePtr input_transformed (new PointCloudSource);
nr_iterations_ = 0;
converged_ = false;
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// If the guessed transformation is non identity
if (guess != Matrix4::Identity ())
{
input_transformed->resize (input_->size ());
// Apply guessed transformation prior to search for neighbours
transformCloud (*input_, *input_transformed, guess);
}
else
*input_transformed = *input_;
transformation_ = Matrix4::Identity ();
// Make blobs if necessary
determineRequiredBlobData ();
PCLPointCloud2::Ptr target_blob (new PCLPointCloud2);
if (need_target_blob_)
pcl::toPCLPointCloud2 (*target_, *target_blob);
// Pass in the default target for the Correspondence Estimation/Rejection code
correspondence_estimation_->setInputTarget (target_);
if (correspondence_estimation_->requiresTargetNormals ())
correspondence_estimation_->setTargetNormals (target_blob);
// Correspondence Rejectors need a binary blob
for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
{
registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
if (rej->requiresTargetPoints ())
rej->setTargetPoints (target_blob);
if (rej->requiresTargetNormals () && target_has_normals_)
rej->setTargetNormals (target_blob);
}
convergence_criteria_->setMaximumIterations (max_iterations_);
convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
// Repeat until convergence
do
{
// Get blob data if needed
PCLPointCloud2::Ptr input_transformed_blob;
if (need_source_blob_)
{
input_transformed_blob.reset (new PCLPointCloud2);
toPCLPointCloud2 (*input_transformed, *input_transformed_blob);
}
// Save the previously estimated transformation
previous_transformation_ = transformation_;
// Set the source each iteration, to ensure the dirty flag is updated
correspondence_estimation_->setInputSource (input_transformed);
if (correspondence_estimation_->requiresSourceNormals ())
correspondence_estimation_->setSourceNormals (input_transformed_blob);
// Estimate correspondences
if (use_reciprocal_correspondence_)
correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
else
correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
//if (correspondence_rejectors_.empty ())
CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
{
registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
if (rej->requiresSourcePoints ())
rej->setSourcePoints (input_transformed_blob);
if (rej->requiresSourceNormals () && source_has_normals_)
rej->setSourceNormals (input_transformed_blob);
rej->setInputCorrespondences (temp_correspondences);
rej->getCorrespondences (*correspondences_);
// Modify input for the next iteration
if (i < correspondence_rejectors_.size () - 1)
*temp_correspondences = *correspondences_;
}
size_t cnt = correspondences_->size ();
// Check whether we have enough correspondences
if (static_cast<int> (cnt) < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
converged_ = false;
break;
}
// Estimate the transform
transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
// Tranform the data
transformCloud (*input_transformed, *input_transformed, transformation_);
// Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
++nr_iterations_;
// Update the vizualization of icp convergence
//if (update_visualizer_ != 0)
// update_visualizer_(output, source_indices_good, *target_, target_indices_good );
converged_ = static_cast<bool> ((*convergence_criteria_));
}
while (!converged_);
// Transform the input cloud using the final transformation
PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n",
final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
// Copy all the values
output = *input_;
// Transform the XYZ + normals
transformCloud (*input_, output, final_transformation_);
}
template <typename PointInT, typename PointOutT> void
pcl::ROPSEstimation <PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
if (triangles_.size () == 0)
{
output.points.clear ();
return;
}
buildListOfPointsTriangles ();
//feature size = number_of_rotations * number_of_axis_to_rotate_around * number_of_projections * number_of_central_moments
unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5;
unsigned int number_of_points = static_cast <unsigned int> (indices_->size ());
output.points.resize (number_of_points, PointOutT ());
for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
{
std::set <unsigned int> local_triangles;
std::vector <int> local_points;
getLocalSurface (input_->points[(*indices_)[i_point]], local_triangles, local_points);
Eigen::Matrix3f lrf_matrix;
computeLRF (input_->points[(*indices_)[i_point]], local_triangles, lrf_matrix);
PointCloudIn transformed_cloud;
transformCloud (input_->points[(*indices_)[i_point]], lrf_matrix, local_points, transformed_cloud);
PointInT axis[3];
axis[0].x = 1.0f; axis[0].y = 0.0f; axis[0].z = 0.0f;
axis[1].x = 0.0f; axis[1].y = 1.0f; axis[1].z = 0.0f;
axis[2].x = 0.0f; axis[2].y = 0.0f; axis[2].z = 1.0f;
std::vector <float> feature;
for (unsigned int i_axis = 0; i_axis < 3; i_axis++)
{
float theta = step_;
do
{
//rotate local surface and get bounding box
PointCloudIn rotated_cloud;
Eigen::Vector3f min, max;
rotateCloud (axis[i_axis], theta, transformed_cloud, rotated_cloud, min, max);
//for each projection (XY, XZ and YZ) compute distribution matrix and central moments
for (unsigned int i_proj = 0; i_proj < 3; i_proj++)
{
Eigen::MatrixXf distribution_matrix;
distribution_matrix.resize (number_of_bins_, number_of_bins_);
getDistributionMatrix (i_proj, min, max, rotated_cloud, distribution_matrix);
std::vector <float> moments;
computeCentralMoments (distribution_matrix, moments);
feature.insert (feature.end (), moments.begin (), moments.end ());
}
theta += step_;
} while (theta < 90.0f);
}
float norm = 0.0f;
for (unsigned int i_dim = 0; i_dim < feature_size; i_dim++)
norm += std::abs (feature[i_dim]);
if (norm < std::numeric_limits <float>::epsilon ())
norm = 1.0f;
else
norm = 1.0f / norm;
for (unsigned int i_dim = 0; i_dim < feature_size; i_dim++)
output.points[i_point].histogram[i_dim] = feature[i_dim] * norm;
}
}
void PointCloudDisplay::incomingCloudCallback()
{
transformCloud();
}
void LaserScanVisualizer::incomingCloudCallback()
{
transformCloud( cloud_message_ );
}
PCPtr getCloudWithoutMutex(const ofVec3f& min, const ofVec3f& max, int stride)
{
if (min.x < 0 || min.y < 0
|| max.x > KINECT_DEFAULT_WIDTH || max.y > KINECT_DEFAULT_HEIGHT
|| min.x > max.x || min.y > max.y)
{
PCL_ERROR ("Wrong arguments to obtain the cloud\n");
return PCPtr(new PC());
}
float voxelSize = mapinect::Constants::CLOUD_VOXEL_SIZE;
if(mapinect::IsFeatureUniformDensityActive())
{
voxelSize = mapinect::Constants::CLOUD_VOXEL_SIZE_FOR_STRIDE(stride);
stride = 1;
}
const int minStride = ::min(mapinect::Constants::CLOUD_STRIDE_H, mapinect::Constants::CLOUD_STRIDE_V);
const int hStride = stride + mapinect::Constants::CLOUD_STRIDE_H - 1;
const int vStride = stride + mapinect::Constants::CLOUD_STRIDE_V - 1;
// Allocate space for max points available
PCPtr partialCloud(new PC());
partialCloud->width = ceil((max.x - min.x) / hStride);
partialCloud->height = ceil((max.y - min.y) / vStride);
partialCloud->is_dense = false;
partialCloud->points.resize(partialCloud->width * partialCloud->height);
register float* depthMap = gKinect->getDistancePixels();
// Iterate over the image's rectangle with step = stride
register int depthIndex = 0;
int cloudIndex = 0;
for(int v = min.y; v < max.y; v += vStride) {
for(register int u = min.x; u < max.x; u += hStride) {
depthIndex = v * KINECT_DEFAULT_WIDTH + u;
PCXYZ& pt = partialCloud->points[cloudIndex];
cloudIndex++;
// Check for invalid measures
if(depthMap[depthIndex] == 0)
{
pt.x = pt.y = pt.z = 0;
}
else
{
ofVec3f pto = gKinect->getWorldCoordinateFor(u,v);
if(pto.z > mapinect::Constants::CLOUD_Z_MAX)
{
pt.x = pt.y = pt.z = 0;
}
else
{
pt.x = pto.x;
pt.y = pto.y;
pt.z = pto.z;
}
}
}
}
if(mapinect::IsFeatureUniformDensityActive())
{
PCPtr preFilter(new PC(*partialCloud));
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (preFilter);
sor.setLeafSize (voxelSize, voxelSize, voxelSize);
sor.filter(*partialCloud);
}
return transformCloud(partialCloud, gTransformation->getWorldTransformation());
}
void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
{
// Add new clouds...
for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
{
int id = map.nodes[i].id;
if(cloud_infos_.find(id) == cloud_infos_.end())
{
// Cloud not added to RVIZ, add it!
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
if(!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() &&
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection()))
{
cv::Mat image, depth;
s.sensorData().uncompressData(&image, &depth, 0);
if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = rtabmap::util3d::cloudRGBFromSensorData(
s.sensorData(),
cloud_decimation_->getInt(),
cloud_max_depth_->getFloat(),
cloud_voxel_size_->getFloat());
if(cloud->size())
{
if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
{
cloud = rtabmap::util3d::passThrough(cloud, "z",
cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*cloud, *cloudMsg);
cloudMsg->header = map.header;
CloudInfoPtr info(new CloudInfo);
info->message_ = cloudMsg;
info->pose_ = rtabmap::Transform::getIdentity();
info->id_ = id;
if (transformCloud(info, true))
{
boost::mutex::scoped_lock lock(new_clouds_mutex_);
new_cloud_infos_.insert(std::make_pair(id, info));
}
}
}
}
}
}
// Update graph
std::map<int, rtabmap::Transform> poses;
for(unsigned int i=0; i<map.graph.posesId.size() && i<map.graph.poses.size(); ++i)
{
poses.insert(std::make_pair(map.graph.posesId[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
}
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
{
poses = rtabmap::graph::radiusPosesFiltering(poses,
node_filtering_radius_->getFloat(),
node_filtering_angle_->getFloat()*CV_PI/180.0);
}
{
boost::mutex::scoped_lock lock(current_map_mutex_);
current_map_ = poses;
}
}
