void DistanceSensorItem::range_cb(const sensor_msgs::Range::ConstPtr &msg)
{
uint8_t type = 0;
uint8_t covariance_ = 0;
if (covariance > 0) covariance_ = covariance;
else covariance_ = uint8_t(calculate_variance(msg->range) * 1E2); // in cm
/** @todo Propose changing covarince from uint8_t to float */
ROS_DEBUG_NAMED("distance_sensor", "DS: %d: sensor variance: %f", sensor_id, calculate_variance(msg->range) * 1E2);
// current mapping, may change later
if (msg->radiation_type == sensor_msgs::Range::INFRARED)
type = MAV_DISTANCE_SENSOR_LASER;
else if (msg->radiation_type == sensor_msgs::Range::ULTRASOUND)
type = MAV_DISTANCE_SENSOR_ULTRASOUND;
owner->distance_sensor(
msg->header.stamp.toNSec() / 1000000,
msg->min_range / 1E-2,
msg->max_range / 1E-2,
msg->range / 1E-2,
type,
sensor_id,
orientation,
covariance_);
}
/**
* Returns the standard deviation for the numbers in the given array.
*
* NOTE: This expects a sorted array!
*/
float calculate_standard_deviation(float data_array[], size_t array_size, int data_position, bool print_value)
{
float variance = calculate_variance(data_array, array_size, data_position, false),
standard_deviation = pow(variance, 0.5);
if (print_value)
printf("%5.2lf", standard_deviation);
return standard_deviation;
}
typename std::iterator_traits<iterator>::value_type calculate_mean_stddev( iterator begin, iterator end, typename std::iterator_traits<iterator>::value_type mean )
{
typedef typename std::iterator_traits<iterator>::value_type value_t;
typedef typename std::iterator_traits<iterator>::difference_type diff_t;
diff_t length = end - begin;
value_t tmp = calculate_variance( begin, end, mean );
if ( length > 1 )
tmp /= length;
return std::sqrt( tmp );
}
void
PeriodicPredictor::calculate_autocorrelation(std::vector<double> *container) {
std::vector<double> samples_copy;
std::deque<double>::iterator it;
for (it = this->samples.begin(); it != this->samples.end(); it++) {
samples_copy.push_back(*it);
}
for (int32_t offset = 0; offset <= this->sample_size/2; offset++) {
std::vector<double> container1(samples_copy.begin(),
samples_copy.end() - offset);
std::vector<double> container2(samples_copy.begin() + offset,
samples_copy.end());
double variance1 = calculate_variance(container1);
double variance2 = calculate_variance(container2);
double covariance = calculate_covariance(container1, container2);
double correlation = (variance1 == 0 || variance2 == 0) ?
1.0 : covariance/(variance1*variance2);
container->push_back(correlation);
}
}
//======================================================================
double simple_image::calculate_pixelvalue_standard_deviation(){
double fraction_of_pixels_to_test = 0.0001;
int number_of_pixels_in_image = image_matrix.rows*image_matrix.cols;
int number_of_pixels_to_test = (int)
ceil((double)number_of_pixels_in_image*fraction_of_pixels_to_test);
//======================================================================
if(verbosity){
std::cout<<"simple_image -> calculate_pixelvalue_standard_deviation() ->";
std::cout<<"testing "<<number_of_pixels_to_test<<" pixels of ";
std::cout<<image_matrix.rows<<"x"<<image_matrix.cols<<"pixels.";
std::cout<<std::endl;
}
cv::Mat image_to_check;
image_matrix.copyTo(image_to_check);
cv::cvtColor(image_to_check,image_to_check, CV_RGB2GRAY);
std::vector<double> list_of_pixel_differences;
cv::RNG OpenCVPseudoRandomNumberGenerator( 0xFFFFFFFF );
for(
int pixel_itterator = 0;
pixel_itterator<number_of_pixels_to_test;
pixel_itterator++)
{
cv::Point point_1;
point_1.y = OpenCVPseudoRandomNumberGenerator.
uniform(0,image_matrix.rows);
point_1.x = OpenCVPseudoRandomNumberGenerator.
uniform(0,image_matrix.cols);
cv::Point point_2;
point_2.y = OpenCVPseudoRandomNumberGenerator.
uniform(0,image_matrix.rows);
point_2.x = OpenCVPseudoRandomNumberGenerator.
uniform(0,image_matrix.cols);
cv::Scalar intensity_1 =
image_matrix.at<uchar>(point_1);
cv::Scalar intensity_2 =
image_matrix.at<uchar>(point_2);
//~ std::cout<<"test ("<<point_1.x<<"|"<<point_1.y<<")";
//~ std::cout<<":("<<intensity_1[0]<<"|"<<intensity_1[1]<<"|"<<intensity_1[2]<< ")";
//~ std::cout<<" and ("<<point_2.x<<"|"<<point_2.y<<")";
//~ std::cout<<":("<<intensity_2[0]<<"|"<<intensity_2[1]<<"|"<<intensity_2[2]<< ")";
//~ std::cout<<std::endl;
double pixel_flux_differences =
abs((double)intensity_1[0]-(double)intensity_2[0]);
//~ std::cout<<"intensity diff R:"<<pixel_flux_difference_in_R;
//~ std::cout<<"B:"<<pixel_flux_difference_in_B;
//~ std::cout<<"G:"<<pixel_flux_difference_in_G;
//~ std::cout<<std::endl;
list_of_pixel_differences.
push_back(pixel_flux_differences);
//~ std::cout<<"intensity diff R:";
//~ std::cout<<list_of_pixel_differences_in_R.at(pixel_itterator);
//~ std::cout<<"B:"<<list_of_pixel_differences_in_B.at(pixel_itterator);
//~ std::cout<<"G:"<<list_of_pixel_differences_in_G.at(pixel_itterator);
//~ std::cout<<std::endl;
}
// Automatic recognition and calibration of
// astronomical images
// thesis of Dustin Lang
double variance_of_pixel_differences =
calculate_variance(&list_of_pixel_differences);
double sigma_in_pxv =
sqrt(variance_of_pixel_differences/2.0);
if(verbosity){
std::cout<<"simple_image -> calculate_pixelvalue_standard_deviation() ->";
std::cout<<"sigma "<<sigma_in_pxv<<"pxv"<<std::endl;
}
return sigma_in_pxv;
}
typename std::iterator_traits<iterator>::value_type calculate_stddev( iterator begin, iterator end )
{
typedef typename std::iterator_traits<iterator>::value_type value_t;
value_t mean = calculate_mean( begin, end );
return std::sqrt( calculate_variance( begin, end, mean ) );
}
typename std::iterator_traits<iterator>::value_type calculate_stddev( iterator begin, iterator end, typename std::iterator_traits<iterator>::value_type mean )
{
return std::sqrt( calculate_variance( begin, end, mean ) );
}