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_);
}
Пример #2
0
/**
 * 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;
}
Пример #3
0
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);
  }
}
Пример #5
0
//======================================================================
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;
}
Пример #6
0
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 ) );
}
Пример #7
0
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 ) );
}