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; } }