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