void ObservationDirectionDataPointsFilter<T>::inPlaceFilter(DataPoints& cloud)
{
const int dim(cloud.features.rows() - 1);
const int featDim(cloud.features.cols());
if (dim != 2 && dim != 3)
{
throw InvalidField(
(boost::format("ObservationDirectionDataPointsFilter: Error, works only in 2 or 3 dimensions, cloud has %1% dimensions.") % dim).str()
);
}
Vector center(dim);
center[0] = centerX;
center[1] = centerY;
if (dim == 3)
center[2] = centerZ;
cloud.allocateDescriptor("observationDirections", dim);
BOOST_AUTO(observationDirections, cloud.getDescriptorViewByName("observationDirections"));
for (int i = 0; i < featDim; ++i)
{
// Check normal orientation
const Vector p(cloud.features.block(0, i, dim, 1));
observationDirections.col(i) = center - p;
}
}
void SimpleSensorNoiseDataPointsFilter<T>::inPlaceFilter(DataPoints& cloud)
{
cloud.allocateDescriptor("simpleSensorNoise", 1);
BOOST_AUTO(noise, cloud.getDescriptorViewByName("simpleSensorNoise"));
switch(sensorType)
{
case 0: // Sick LMS-1xx
{
noise = computeLaserNoise(0.012, 0.0068, 0.0008, cloud.features);
break;
}
case 1: // Hokuyo URG-04LX
{
noise = computeLaserNoise(0.028, 0.0013, 0.0001, cloud.features);
break;
}
case 2: // Hokuyo UTM-30LX
{
noise = computeLaserNoise(0.018, 0.0006, 0.0015, cloud.features);
break;
}
case 3: // Kinect / Xtion
{
const int dim = cloud.features.rows();
const Matrix squaredValues(cloud.features.topRows(dim-1).colwise().norm().array().square());
noise = squaredValues*(0.5*0.00285);
break;
}
case 4: // Sick Tim3xx
{
noise = computeLaserNoise(0.004, 0.0053, -0.0092, cloud.features);
break;
}
default:
throw InvalidParameter(
(boost::format("SimpleSensorNoiseDataPointsFilter: Error, cannot compute noise for sensorType id %1% .") % sensorType).str());
}
}
