void TestInterpolation<T>::testCollectInterpolants() { USING_NK_NS USING_NKHIVE_NS // construct volume T default_val(1); vec3d res(1.0); vec3d kernel_offset(0.5); typename Volume<T>::shared_ptr volume( new Volume<T>(1, 2, default_val, res, kernel_offset)); // easy to track values i32 value=0; for (i32 z=0; z<4; ++z) { for (i32 y=0; y<4; ++y) { for (i32 x=0; x<4; ++x) { volume->set(x,y,z,value++); } } } vec3i min_indices(0,0,0); vec3i max_indices(3,3,3); CubicInterpolation<T> cubic_interp(volume); // test interior typename CubicInterpolation<T>::calc_type interpolants[64]; cubic_interp.collectInterpolants(min_indices, max_indices, interpolants); for (i32 i=0; i<64; ++i) { CPPUNIT_ASSERT(interpolants[i] == typename CubicInterpolation<T>::calc_type(i)); } // test boundary min_indices = vec3i(0,0,1); max_indices = vec3i(3,3,4); cubic_interp.collectInterpolants(min_indices, max_indices, interpolants); for (i32 i=0; i<48; ++i) { CPPUNIT_ASSERT(interpolants[i] == typename CubicInterpolation<T>::calc_type(i + 16)); } for (i32 i=0; i<16; ++i) { CPPUNIT_ASSERT(interpolants[i + 48] == typename CubicInterpolation<T>::calc_type(1)); } }
/// /// \brief Vespucci::Math::Quantification::FindBandwidth /// \param X /// \param min_index /// \param max_index /// \param midline /// \param abscissa_step /// \return /// Finds the full-width at half maximum of a peak bound by min_index and max_index double Vespucci::Math::Quantification::FindBandwidth(const arma::vec &X, arma::uword min_index, arma::uword max_index, arma::vec &midline, arma::vec &baseline, double abscissa_step) { arma::vec region = X.subvec(min_index, max_index); arma::uword size = region.n_elem; double maximum, half_maximum; double start_value, end_value; midline.set_size(size); max_index = 0; arma::uword left_index = 0; arma::uword right_index = 0; start_value = X(min_index); end_value = X(max_index); baseline = arma::linspace(start_value, end_value, size); region -= baseline; maximum = region.max(); half_maximum = maximum / 2.0; arma::uvec max_indices = find(region == maximum); max_index = max_indices(0); //search for left inflection point for (arma::uword i = max_index; i > 0; --i){ if (X(i) - half_maximum < 0){ left_index = i; break; } } //search for right inflection point for (arma::uword i = max_index; i < size; ++i){ if (X(i) - half_maximum < 0){ right_index = i; break; } } //check to make sure the values on the other side of the inflection point aren't better if (left_index > 0 && right_index < size - 1){ if(std::fabs(X(left_index) - half_maximum) > std::fabs(X(left_index - 1) - half_maximum)){ --left_index; } if (std::fabs(X(right_index) - half_maximum) > std::fabs(X(right_index + 1) - half_maximum)){ ++right_index; } } double region_size = region.subvec(left_index, right_index).n_elem; return abscissa_step * region_size; }
void laserPointsCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_in) { tf::StampedTransform laser_transform; if(!tf_listener->waitForTransform( "/map", "/head_hokuyo_frame", cloud_in->header.stamp, ros::Duration(1.0))) { ROS_WARN("Transform from /map to /head_hokuyo_frame failed"); return; } tf_listener->lookupTransform("/map", "/head_hokuyo_frame", cloud_in->header.stamp, laser_transform); octomap::point3d laser_origin = octomap::pointTfToOctomap(laser_transform.getOrigin()); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_raw_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*cloud_in, *pcl_raw_cloud); // filter out the points on the robot pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_robot_filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ>()); range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr ( new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::GE, 0.25))); range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr ( new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::LE, -0.25))); range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr ( new pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::GE, 0.25))); range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr ( new pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::LE, -0.25))); range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr ( new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GE, 1.2))); range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr ( new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LE, -.1))); pcl::ConditionalRemoval<pcl::PointXYZ> robot_filter(range_cond); robot_filter.setInputCloud(pcl_raw_cloud); robot_filter.setKeepOrganized(true); robot_filter.filter(*pcl_robot_filtered_cloud); ROS_DEBUG("pcl_robot_filtered_cloud size: %lu", pcl_robot_filtered_cloud->points.size()); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_map_cloud(new pcl::PointCloud<pcl::PointXYZ>); if(!tf_listener->waitForTransform( "/map", "/base_link", cloud_in->header.stamp, ros::Duration(1.0))) { ROS_WARN("Transform from /map to /base_link failed"); return; } pcl_ros::transformPointCloud("/map", cloud_in->header.stamp, *pcl_robot_filtered_cloud, "/base_link", *pcl_map_cloud, *tf_listener); // filter out the ground pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ground_filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PassThrough<pcl::PointXYZ> ground_filter; ground_filter.setInputCloud(pcl_map_cloud); ground_filter.setFilterFieldName("z"); ground_filter.setFilterLimits(-1, .05); ground_filter.setFilterLimitsNegative(true); ground_filter.filter(*pcl_ground_filtered_cloud); ROS_DEBUG("pcl_ground_filtered_cloud size: %lu", pcl_ground_filtered_cloud->points.size()); // filter out the max range readings pcl::PointIndices::Ptr max_indices(new pcl::PointIndices); int i=0; for(pcl::PointCloud<pcl::PointXYZ>::const_iterator point = pcl_ground_filtered_cloud->points.begin(); point != pcl_ground_filtered_cloud->points.end(); point++) { // if this point is within .03 m of the max sensor reading then we want to remove it double x = point->x - laser_origin.x(); double y = point->y - laser_origin.y(); double z = point->z - laser_origin.z(); if(fabs(sqrt(x*x + y*y + z*z) - 30.0) < .04) max_indices->indices.push_back(i); i++; } pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_max_filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::ExtractIndices<pcl::PointXYZ> max_filter; max_filter.setInputCloud(pcl_ground_filtered_cloud); max_filter.setKeepOrganized(true); max_filter.setNegative(true); max_filter.setIndices(max_indices); max_filter.filter(*pcl_max_filtered_cloud); ROS_DEBUG("pcl_max_filtered_cloud size: %lu", pcl_max_filtered_cloud->points.size()); // convert to ros datatype as an intermediate step to converting to octomap type sensor_msgs::PointCloud2 filtered_cloud; pcl::toROSMsg(*pcl_max_filtered_cloud, filtered_cloud); ROS_DEBUG("filtered_cloud size: %lu", filtered_cloud.data.size()); // convert to octomap type octomap::Pointcloud octomap_cloud; octomap::pointCloud2ToOctomap(filtered_cloud, octomap_cloud); ROS_DEBUG("octomap pointcloud size: %lu", octomap_cloud.size()); tree->insertPointCloudRays(octomap_cloud, laser_origin, -1, true); ROS_DEBUG("map_changed is now true"); map_changed = true; }