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