/** \brief accumulate and log message latency, dropped messages */
void checkLatency(const roslib::Header *hdr)
{
static bool firstMsg = true; // have we seen any yet?
static uint32_t nextSeq = 0; // next sequence number expected
// accumulate latency mean and variance
double timeDelay = ros::Time::now().toSec() - hdr->stamp.toSec();
latency.addSample(timeDelay);
ROS_DEBUG("message %u received, latency: %.3f seconds",
hdr->seq, timeDelay);
// check for lost messages
if (firstMsg)
{
firstMsg = false;
}
else if (nextSeq < hdr->seq)
{
// If the publisher terminates and then restarts, the sequence
// numbers apparently reset. We don't count that as a dropped
// message.
uint32_t missed = hdr->seq - nextSeq;
if (droppedMsgs == 0)
// only log INFO message the first time
ROS_INFO("%u Velodyne messages dropped", missed);
else
// log DEBUG message for any remaining losses
ROS_DEBUG("%u more Velodyne messages dropped", missed);
droppedMsgs += missed;
}
nextSeq = hdr->seq + 1;
}
bool numaGetStdDeviation(Numa* na, l_float32* stdDev, l_float32* errorPercentage, l_float32* mean) {
l_int32 n = numaGetCount(na);
if (n < 2) {
return false;
}
l_int32 val;
RunningStats stats;
for (int i = 0; i < n; i++) {
numaGetIValue(na, i, &val);
stats.Push(val);
}
if (stdDev != NULL) {
*stdDev = stats.PopulationStandardDeviation();
}
if(errorPercentage!=NULL){
if(stats.Mean()>0){
*errorPercentage = stats.PopulationStandardDeviation() / fabs(stats.Mean());
} else {
*errorPercentage = 0;
}
}
if(mean!=NULL){
*mean = stats.Mean();
}
return true;
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, NODE);
ros::NodeHandle node;
if (0 != getParameters(argc, argv))
return 9;
if (0 != data->setup())
return 2;
// subscribe to velodyne input -- make sure queue depth is minimal,
// so any missed scans are discarded. Otherwise latency gets out of
// hand. It's bad enough anyway.
ros::Subscriber velodyne_scan =
node.subscribe("velodyne/rawscan", qDepth,
&velodyne::Data::processRawScan, (velodyne::Data *) data,
ros::TransportHints().tcpNoDelay(true));
output = node.advertise<sensor_msgs::PointCloud>("velodyne/hybridcloud",
qDepth);
// preallocate the anticipated amount of space for the point cloud
pc.points.resize(velodyne::SCANS_PER_REV);
ROS_INFO(NODE ": starting main loop");
ros::spin(); // handle incoming data
ROS_INFO(NODE ": exiting main loop");
ROS_INFO("message latency: %.3f +/- %.3f",
latency.getMean(), latency.getStandardDeviation());
if (droppedMsgs)
ROS_INFO("total of %u Velodyne messages dropped", droppedMsgs);
else
ROS_INFO("no Velodyne messages dropped");
data->shutdown();
delete data;
return 0;
}
l_float32 numaGetMeanHorizontalCrossingWidths(Numa* nay){
l_int32 first, last;
RunningStats stats;
numaGetNonzeroRange(nay,0,&first, &last);
l_int32 val;
l_int32 count = 0;
for(int i = first; i<last;i++){
numaGetIValue(nay,i,&val);
if(val==0){
count++;
}
if(count>0 && val>0){
stats.Push(count);
count = 0;
}
}
if(count>0){
stats.Push(count);
}
return stats.Mean();
}
Pix* PixBlurDetect::pixMakeBlurMask(Pix* pixGrey, Pix* pixMedian, l_float32* blurValue, Pix** pixBinary) {
l_int32 width, height, wpld, wplbx, wplm, wplby;
l_int32 y, x;
l_uint32 *datad, *databx, *datam, *databy, *linebx, *linem, *lined;
width = pixGetWidth(pixMedian);
height = pixGetHeight(pixMedian);
Pix* blurMeasure = pixCreate(width, height, 8);
l_int32 vertRating, horzRating;
Pix* pixBinaryy = makeEdgeMask(pixMedian, L_VERTICAL_EDGES, &vertRating);
Pix* pixBinaryx = makeEdgeMask(pixMedian, L_HORIZONTAL_EDGES, &horzRating);
datad = pixGetData(blurMeasure);
databx = pixGetData(pixBinaryx);
databy = pixGetData(pixBinaryy);
datam = pixGetData(pixMedian);
wpld = pixGetWpl(blurMeasure);
wplbx = pixGetWpl(pixBinaryx);
wplby = pixGetWpl(pixBinaryy);
wplm = pixGetWpl(pixMedian);
RunningStats stats;
if (vertRating < horzRating) {
if (mDebug) {
printf("measuring from left to right\n");
}
if (pixBinary != NULL) {
*pixBinary = pixCopy(NULL, pixBinaryy);
}
for (y = 1; y < height - 1; y++) {
linem = datam + y * wplm;
lined = datad + y * wpld;
linebx = databy + y * wplby;
for (x = 1; x < width - 1; x++) {
bool hasx = !GET_DATA_BIT(linebx, x);
if (hasx) {
l_int32 right;
pixGetLastOffPixelInRun(pixBinaryy, x, y, L_FROM_LEFT, &right);
l_uint8 edgeWidth = (right - x) + 1;
l_uint8 leftColor = GET_DATA_BYTE(linem, right + 1);
l_uint8 rightColor = GET_DATA_BYTE(linem, x - 1);
int intensity = abs((int) (rightColor - leftColor));
double slope = (intensity / edgeWidth) / 255.0;
//printf("(%i,%i) with=%i, intensity diff = %i, slope=%f\n", x,y, edgeWidth, intensity, slope);
stats.Push(slope);
x = right;
double val = convertSlopeToBlurIndicator(slope);
SET_DATA_BYTE(lined, x, val);
}
}
}
} else {
if (mDebug) {
printf("measuring from top to bottom\n");
}
if (pixBinary != NULL) {
*pixBinary = pixCopy(NULL, pixBinaryx);
}
for (x = 1; x < width - 1; x++) {
for (y = 1; y < height - 1; y++) {
bool hasy = !GET_DATA_BIT(databx + y * wplbx, x);
if (hasy) {
l_int32 bottom;
pixGetLastOffPixelInRun(pixBinaryx, x, y, L_FROM_TOP, &bottom);
l_uint8 edgeWidth = (bottom - y) + 1;
l_uint8 leftColor = GET_DATA_BYTE(datam + (y - 1) * wplm, x);
l_uint8 rightColor = GET_DATA_BYTE(datam + (bottom + 1) * wplm, x);
int intensity = abs((int) (rightColor - leftColor));
double slope = (intensity / edgeWidth) / 255.0;
stats.Push(slope);
//printf("(%i,%i) with=%i, intensity diff = %i, slope=%f\n", x,y, edgeWidth, intensity, slope);
for (int i = y; i <= bottom; i++) {
SET_DATA_BIT(databx + i * wplbx, x);
}
double val = convertSlopeToBlurIndicator(slope);
SET_DATA_BYTE(datad + y * wpld, x, val);
}
}
}
}
if (blurValue != NULL) {
*blurValue = stats.Mean();
}
pixDestroy(&pixBinaryx);
pixDestroy(&pixBinaryy);
return blurMeasure;
}
bool checkTextConditions(Numa* extrema, Numa* numaPixelSum, l_float32* textPropability, l_float32* textSize, bool debug) {
//1 group of similar line lengths (peak height)
//2 group of similar white space lengths (valley height)
//3 group of similar line heights (area under peak -> line thickness)
//4 number of crossings is around double the number of peaks
//5 similar distance between crossings
bool success = false;
//split extrema into peaks and valleys
Numa *px, *py, *vx, *vy, *pa, *errors, *paw;
numaSplitExtrema(extrema, numaPixelSum, &px, &py, &vx, &vy, &pa, &paw, debug);
errors = numaCreate(5);
RunningStats stats;
//check point 1
//height of peaks corresponds to height of text lines. they should be similar
l_float32 lineLengthDeviation, lineLengthError = 0;
l_float32 lineLengthMean;
success = numaGetStdDeviation(py, &lineLengthDeviation, &lineLengthError,&lineLengthMean);
if (!success) {
return false;
}
numaAddNumber(errors, lineLengthError);
stats.Push(lineLengthError);
//check point 2
//height valleys should be similar
l_float32 spacingLengthDeviation, spacingLengthError = 0;
success = numaGetStdDeviation(vy, &spacingLengthDeviation, &spacingLengthError,NULL);
if (!success) {
return false;
}
if(lineLengthMean>0){
spacingLengthError = spacingLengthDeviation/lineLengthMean;
}
numaAddNumber(errors, spacingLengthError);
stats.Push(spacingLengthError);
//check point 3
//the distance between lines is indicated by the distance of the peaks which is the delta
Numa* peakDelta = numaMakeDelta3(px);
l_float32 lineHeightDeviation, lineHeightError;
success = numaGetStdDeviation(peakDelta, &lineHeightDeviation, &lineHeightError,NULL);
if (!success) {
return false;
}
numaAddNumber(errors, lineHeightError);
stats.Push(lineHeightError);
//still checking point 3
l_float32 peakAreaDeviation, peakAreaError;
success = numaGetStdDeviation(pa, &peakAreaDeviation, &peakAreaError,NULL);
if (!success) {
return false;
}
//printNuma(pa,"peak areas");
numaAddNumber(errors, peakAreaError);
stats.Push(peakAreaError);
//check point 4
//first find the max valley and min peak
l_float32 maxValleyY, minPeakY;
l_int32 maxValleyX, minPeakX;
numaGetMax(vy, &maxValleyY, &maxValleyX);
numaGetMin(py, &minPeakY, &minPeakX);
//printf("min peak = %f\nmax valley = %f\n", minPeakY, maxValleyY);
l_int32 firstCrossingY = maxValleyY + (minPeakY - maxValleyY) / 3;
l_int32 secondCrossingY = minPeakY - (minPeakY - maxValleyY) / 3;
//get the crossings
Numa* crossings1 = numaCrossingsByThreshold(NULL, numaPixelSum, firstCrossingY);
Numa* crossings2 = numaCrossingsByThreshold(NULL, numaPixelSum, secondCrossingY);
//number of crossings should be around double the number of peaks
l_int32 p2 = numaGetCount(px) * 2;
l_int32 c1n = numaGetCount(crossings1);
l_int32 c2n = numaGetCount(crossings1);
l_float32 thresholdCrossingDeviation = sqrt(((p2 - c1n) * (p2 - c1n) + (p2 - c2n) * (p2 - c2n)) / 2);
l_float32 thresholdCrossingError = thresholdCrossingDeviation/((c1n+c2n)/2);
numaAddNumber(errors, thresholdCrossingError);
stats.Push(thresholdCrossingError);
//check point 6 maybe later
if(debug) {
printf("threshold deviation = %f\t\terror = %f\n", thresholdCrossingDeviation,thresholdCrossingError);
printf("peak area deviation = %f\t\terror = %f\n", peakAreaDeviation,peakAreaError);
printf("line distance deviation = %f\terror = %f\n", lineHeightDeviation,lineHeightError);
printf("spacing length deviation = %f\terror = %f\n", spacingLengthDeviation,spacingLengthError);
printf("line length deviation = %f\terror = %f\n", lineLengthDeviation,lineLengthError);
}
//calculate deviation from optimum values
// l_float32 prob = sqrt(((lineLengthError * lineLengthError)*.15 + (spacingLengthError*spacingLengthError)*.15 + (lineHeightError * lineHeightError)*.15 + (peakAreaError * peakAreaError)*35
// + (thresholdCrossingError * thresholdCrossingError)*.2));
l_float32 totalError = 0;
for(int i = 0; i < 5; i++){
l_float32 error;
numaGetFValue(errors, i,&error);
error = 1 - error;
totalError+= (1 - error)*(1 - error);
}
totalError=sqrt(totalError/5);
if (textPropability != NULL) {
*textPropability = totalError;
}
if(textSize!=NULL){
RunningStats stats;
l_int32 w;
l_int32 n = numaGetCount(paw);
for(int i = 0; i<n; i++){
numaGetIValue(paw,i,&w);
stats.Push(w);
}
*textSize = stats.Mean();
}
numaDestroy(&crossings1);
numaDestroy(&crossings2);
numaDestroy(&px);
numaDestroy(&py);
numaDestroy(&vx);
numaDestroy(&vy);
numaDestroy(&pa);
numaDestroy(&errors);
numaDestroy(&paw);
// if(debug) {
// printf("spacing dv = %f\nline dv = %f\nspacing height dv = %f\nline height dv = %f\ncrossing deviation = %f\npeak area deviation = %f\ntext propability = %f\n", spacingLengthDeviation, lineLengthDeviation, spaceHeightDeviation, lineHeightDeviation, thresholdCrossingDeviation,peakAreaDeviation, *textPropability);
// }
return true;
}
void numaSplitExtrema(Numa* nax, Numa* nay, Numa** peaksX, Numa** peaksY, Numa** valleysX, Numa** valleysY, Numa** peakAreas, Numa** peakAreaWidths, bool debug) {
Numa* navx;
Numa* napx;
Numa* navy;
Numa* napy;
Numa* napa;
Numa* napaw;
bool isPeakFirst = false, b;
l_int32 n = numaGetCount(nax);
l_int32 first, second, peak_count, valley_count, index;
l_int32 start = 0, end = 0;
l_float32 valX, valY, sum;
if (n < 2) {
return;
}
peak_count = n / 2;
valley_count = peak_count;
if (n % 2 != 0) {
numaGetIValue(nax, 0, &index);
numaGetIValue(nay, index, &first);
numaGetIValue(nax, 1, &index);
numaGetIValue(nay, index, &second);
isPeakFirst = first > second;
if (isPeakFirst) {
valley_count--;
} else {
peak_count--;
}
}
b = isPeakFirst;
navx = numaCreate(valley_count);
napx = numaCreate(peak_count);
navy = numaCreate(valley_count);
napy = numaCreate(peak_count);
napa = numaCreate(peak_count);
napaw = numaCreate(0);
for (int i = 0; i < n; i++) {
numaGetFValue(nax, i, &valX);
numaGetFValue(nay, valX, &valY);
if (b) {
numaAddNumber(napx, valX);
numaAddNumber(napy, valY);
} else {
numaAddNumber(navx, valX);
numaAddNumber(navy, valY);
}
b = !b;
}
//calculate the area under the peaks
n = numaGetCount(nax);
RunningStats stats;
l_int32 i = 0;
while(i<n){
if(i==0 && isPeakFirst){
start = 0;
} else {
numaGetIValue(nax, i++, &start);
}
l_int32 peak;
numaGetIValue(nax,i,&index);
numaGetIValue(nay, index, &peak);
i++;
if(i==n){
end = numaGetCount(nay);;
} else {
numaGetIValue(nax, i, &end);
}
//printf("integrating %i to %i\n",start, end);
l_int32 error = numaGetSumOnInterval(nay,start,end,&sum);
if(!error){
numaAddNumber(napa, sum);
if(debug){
printf("sum = %f, peak = %i, width = %f\n",sum, peak, sum/peak);
}
numaAddNumber(napaw, rintf(sum/peak));
if(i>0 && i<n-1){
stats.Push(sum);
}
}
}
//remove first or last peak area if it differs significantly from the rest (remove text lines that are cut off)
l_float32 mean = stats.Mean();
if(numaGetCount(napa)>1){
numaGetFValue(napa,0,&sum);
l_int32 diff = mean-sum;
//if peak area differs from mean by at least 50% remove it
if((diff/mean)>.3){
if(debug){
printf("removing first peak area diff=%i mean = %f\n",diff,mean);
}
numaRemoveNumber(napa,0);
numaRemoveNumber(napaw,0);
}
}
n = numaGetCount(napa);
if(n>1){
numaGetFValue(napa,n-1,&sum);
l_int32 diff = mean-sum;
//if peak area differs from mean by at least 50% remove it
if((diff/mean)>.3){
if(debug){
printf("removing last peak area diff=%i mean = %f\n",diff,mean);
}
numaRemoveNumber(napa,n-1);
numaRemoveNumber(napaw,n-1);
}
}
*peakAreas = napa;
*peaksX = napx;
*peaksY = napy;
*valleysX = navx;
*valleysY = navy;
*peakAreaWidths = napaw;
}
double vec_mean(const vector<double> &data_vec)
{
RunningStats stats;
stats.add(data_vec);
return stats.comp_mean();
}
void operator() ( const Bar &bar ) {
++cnt;
double t = cnt;
rsUpper.Add( t, bar.High() - bar.Open() );
rsLower.Add( t, bar.Open() - bar.Low() );
}
~CalcSixMonMeans( void ) {
rsUpper.CalcStats();
rsLower.CalcStats();
cout << endl << " upper mean=" << rsUpper.MeanY() << ", SD=" << rsUpper.SD();
cout << endl << " lower mean=" << rsLower.MeanY() << ", SD=" << rsLower.SD();
}
