Got vision working with Ben.
diff --git a/vision/CameraProcessor.cpp b/vision/CameraProcessor.cpp
index e186cbf..56bd36e 100644
--- a/vision/CameraProcessor.cpp
+++ b/vision/CameraProcessor.cpp
@@ -1,4 +1,7 @@
#include "vision/CameraProcessor.h"
+#include "aos/common/logging/logging.h"
+
+//#define LOCAL_DEBUG 1
// create a new target
Target::Target(std::vector<cv::Point> new_contour,
@@ -264,11 +267,11 @@
img_height = height;
buffer_size = img_height * img_width * 3;
#if LOCAL_DEBUG
- global_display = cvCreateImage(cvSize(width, height),
- IPL_DEPTH_8U, 3);
+ global_display = cvCreateImage(cvSize(width, height),
+ IPL_DEPTH_8U, 3);
#endif
grey_image = cvCreateImage(cvSize(width, height),
- IPL_DEPTH_8U, 1);
+ IPL_DEPTH_8U, 1);
grey_mat = new cv::Mat(grey_image);
// calculate a desired histogram before we start
@@ -288,8 +291,8 @@
j++;
}
- src_header_image = cvCreateImage(cvSize(width, height),
- IPL_DEPTH_8U, 3);
+ src_header_image = cvCreateImage(cvSize(width, height),
+ IPL_DEPTH_8U, 3);
}
// throw stuff away
@@ -336,41 +339,52 @@
// by far the longest running function in this code
void ProcessorData::threshold(uchar* buffer) {
#if LOCAL_DEBUG
- uchar * draw_buffer = (uchar *) global_display->imageData;
+ uchar * draw_buffer = (uchar *) global_display->imageData;
#endif
- for (int i = 0,j = 0;
- i < (buffer_size);
- i+= 3,j++) {
- uchar r = buffer[i + 2];
- uchar g = buffer[i + 1];
- uchar b = buffer[i + 0];
- uchar h, s, v;
+ for (int i = 0, j = 0; i < (buffer_size); i+= 3, j++) {
+ uchar r = buffer[i + 2];
+ uchar g = buffer[i + 1];
+ uchar b = buffer[i + 0];
+ uchar h, s, v;
- RGBtoHSV(r, g, b, &h, &s, &v);
+ RGBtoHSV(r, g, b, &h, &s, &v);
- /* if (h == 0 && s == 0 && v >= v1 && v <= v2) {
+ if (g > 128) {
#if LOCAL_DEBUG
- draw_buffer[i + 0] = 200;
- draw_buffer[i + 1] = 50;
- draw_buffer[i + 2] = 100;
+ draw_buffer[i + 0] = 120;
+ draw_buffer[i + 1] = 80;
+ draw_buffer[i + 2] = 70;
#endif
- grey_image->imageData[j] = 255;
- } else */if(h >= h1 && h <= h2 && v >= v1 && v <= v2 && s >= s1 && s <= s2){
+ grey_image->imageData[j] = 255;
+ } else if (h == 0 && s == 0 && v >= v1 && v <= v2) {
+ // Value thresholds invalid pixels.
#if LOCAL_DEBUG
-/* draw_buffer[i + 0] = 255;
- draw_buffer[i + 1] = 0;
- draw_buffer[i + 2] = 0;*/
+ draw_buffer[i + 0] = 200;
+ draw_buffer[i + 1] = 50;
+ draw_buffer[i + 2] = 100;
#endif
- grey_image->imageData[j] = 255;
- }else{
+ grey_image->imageData[j] = 255;
+ } else if (h >= h1 && h <= h2 && v >= v1 &&
+ v <= v2 && s >= s1 && s <= s2){
+ // HSV Thresholded image.
#if LOCAL_DEBUG
-/* draw_buffer[i + 0] = buffer[i + 0];
- draw_buffer[i + 1] = buffer[i + 1];
- draw_buffer[i + 2] = buffer[i + 2];*/
+ draw_buffer[i + 0] = 255;
+ draw_buffer[i + 1] = 0;
+ draw_buffer[i + 2] = 0;
#endif
- grey_image->imageData[j] = 0;
- }
- }
+ grey_image->imageData[j] = 255;
+ } else {
+ // Display the unmodified image.
+#if LOCAL_DEBUG
+ draw_buffer[i + 0] = buffer[i + 0];
+ draw_buffer[i + 1] = buffer[i + 1];
+ draw_buffer[i + 2] = buffer[i + 2];
+#endif
+ grey_image->imageData[j] = 0;
+ }
+
+ }
+
}
// run find contours and try to make them squares
@@ -416,38 +430,37 @@
// filter the contours down to a list of targets
void ProcessorData::filterToTargets() {
- std::vector<std::pair<
- std::vector<cv::Point>,
- std::vector<cv::Point> > >::iterator contour_it;
- for(contour_it=contour_pairs.begin();
- contour_it != contour_pairs.end();
- contour_it++){
- double check = 0;
- std::vector<cv::Point> raw_contour = std::get<0>(*contour_it);
- std::vector<cv::Point> contour = std::get<1>(*contour_it);
- FullRect rect = calcFullRect(&contour);
- if(contour.size() == 4 &&
- cullObvious(rect, contourArea(contour)) &&
- (check = checkHistogram(rect, *grey_mat)) <= HIST_MATCH){
- // now we have a target, try to improve the square
+ std::vector<std::pair<
+ std::vector<cv::Point>,
+ std::vector<cv::Point> > >::iterator contour_it;
+ for(contour_it=contour_pairs.begin();
+ contour_it != contour_pairs.end();
+ contour_it++){
+ double check = 0;
+ std::vector<cv::Point> raw_contour = std::get<0>(*contour_it);
+ std::vector<cv::Point> contour = std::get<1>(*contour_it);
+ FullRect rect = calcFullRect(&contour);
+ if(contour.size() == 4 &&
+ cullObvious(rect, contourArea(contour)) &&
+ (check = checkHistogram(rect, *grey_mat)) <= HIST_MATCH){
+ // now we have a target, try to improve the square
#if LOCAL_DEBUG
- /* printf("________\n");
- printf("\tcont= %d raw= %d\n",
- (int)contour.size(), (int)raw_contour.size());
- std::vector<cv::Point2i>::iterator point_it;
- for(point_it=raw_contour.begin();
- point_it != raw_contour.end(); point_it++){
- printf("(%d,%d)", point_it->x, point_it->y);
- }
- printf("\n");*/
+ /* printf("________\n");
+ printf("\tcont= %d raw= %d\n",
+ (int)contour.size(), (int)raw_contour.size());
+ std::vector<cv::Point2i>::iterator point_it;
+ for(point_it=raw_contour.begin();
+ point_it != raw_contour.end(); point_it++){
+ printf("(%d,%d)", point_it->x, point_it->y);
+ }
+ printf("\n");*/
#endif
- target_list.push_back(Target(contour,
- raw_contour, rect, check, is_90));
- }
- //if(contour.size() == 4 &&
- // cullObvious(rect, contourArea(contour)) ) {
- // printf("check= %.2f\n", check);
- //}
- }
+ target_list.push_back(Target(contour,
+ raw_contour, rect, check, is_90));
+ }
+ if (contour.size() == 4 && cullObvious(rect, contourArea(contour))) {
+ LOG(INFO, "check= %.2f\n", check);
+ }
+ }
}
diff --git a/vision/CameraProcessor.h b/vision/CameraProcessor.h
index a0ff7aa..8d51cd8 100644
--- a/vision/CameraProcessor.h
+++ b/vision/CameraProcessor.h
@@ -12,13 +12,13 @@
// an over described geometric representation of a rectangle
class FullRect {
- public:
- FullRect();
- cv::Point2f ur; // upper right
- cv::Point2f ul; // upper left
- cv::Point2f br; // bottom right
- cv::Point2f bl; // bottom_left
- cv::Point2f centroid; //centroid
+ public:
+ FullRect();
+ cv::Point2f ur; // upper right
+ cv::Point2f ul; // upper left
+ cv::Point2f br; // bottom right
+ cv::Point2f bl; // bottom_left
+ cv::Point2f centroid; //centroid
};
// All data needed once a target is found
@@ -68,7 +68,7 @@
double vert_hist[HIST_SIZE]; // desired vertical histogram
double horz_hist[HIST_SIZE]; // desired horizontal histogram
// defines the minimum dist for a match
- static const double HIST_MATCH = 1.76;
+ static const double HIST_MATCH = 1.9;
double calcHistComponent(
cv::Point2i start,
cv::Point2i end,
diff --git a/vision/GoalMaster.cpp b/vision/GoalMaster.cpp
index c1b45b5..3155cf7 100644
--- a/vision/GoalMaster.cpp
+++ b/vision/GoalMaster.cpp
@@ -37,18 +37,22 @@
sizeof(kPixelsToMeters) / sizeof(kPixelsToMeters[0]),
kPixelsToMeters,
targets->percent_elevation_off_center);
- LOG(DEBUG, "think target is %f meters away\n", meters);
+ const double shooter_speed = interpolate(
+ sizeof(kMetersToShooterSpeeds) / sizeof(kMetersToShooterSpeeds[0]),
+ kMetersToShooterSpeeds,
+ meters);
+ const double shooter_angle = interpolate(
+ sizeof(kMetersToShooterAngles) / sizeof(kMetersToShooterAngles[0]),
+ kMetersToShooterAngles,
+ meters);
+
+ LOG(DEBUG, "think target is %f meters away Speed %f Angle %f\n",
+ meters, shooter_speed, shooter_angle);
target_angle.MakeWithBuilder()
/*.target_angle(angle_goal)*/
- .shooter_speed(interpolate(
- sizeof(kMetersToShooterSpeeds) / sizeof(kMetersToShooterSpeeds[0]),
- kMetersToShooterSpeeds,
- meters))
- .shooter_angle(interpolate(
- sizeof(kMetersToShooterAngles) / sizeof(kMetersToShooterAngles[0]),
- kMetersToShooterAngles,
- meters))
+ .shooter_speed(shooter_speed)
+ .shooter_angle(shooter_angle)
.Send();
}
}
diff --git a/vision/JPEGRoutines.cpp b/vision/JPEGRoutines.cpp
index 54e4042..acddd89 100644
--- a/vision/JPEGRoutines.cpp
+++ b/vision/JPEGRoutines.cpp
@@ -148,7 +148,7 @@
// printf( "Image width and height: %d pixels and %d pixels.\n", cinfo.image_width, cinfo.image_height );
// printf( "Color components per pixel: %d.\n", cinfo.num_components );
// printf( "Color space: %d.\n", cinfo.jpeg_color_space );
- printf("JpegDecompressed\n");
+ //printf("JpegDecompressed\n");
jpeg_start_decompress( &cinfo );
@@ -180,10 +180,10 @@
aos::time::Time timestamp_end = aos::time::Time::Now();
- double jpeg_part = ((timestamp_end - timestamp_start).nsec()) / 1000000000.0;
- double longer_part = ((timestamp_end - timestamp_old).nsec()) / 1000000000.0;
+ //double jpeg_part = ((timestamp_end - timestamp_start).nsec()) / 1000000000.0;
+ //double longer_part = ((timestamp_end - timestamp_old).nsec()) / 1000000000.0;
- printf("%g %g\n",jpeg_part / longer_part,1.0 / longer_part);
+ //printf("%g %g\n",jpeg_part / longer_part,1.0 / longer_part);
timestamp_old = timestamp_end;
diff --git a/vision/OpenCVWorkTask.cpp b/vision/OpenCVWorkTask.cpp
index 5c420d3..df4d450 100644
--- a/vision/OpenCVWorkTask.cpp
+++ b/vision/OpenCVWorkTask.cpp
@@ -14,6 +14,7 @@
#include "aos/atom_code/camera/Buffers.h"
#include "aos/externals/libjpeg/include/jpeglib.h"
#include "aos/atom_code/init.h"
+#include "aos/common/logging/logging.h"
#include "vision/OpenCVWorkTask.h"
#include "vision/CameraProcessor.h"
@@ -22,7 +23,6 @@
#include "vision/JPEGRoutines.h"
-
namespace frc971 {
namespace vision {
@@ -33,6 +33,27 @@
frc971::vision::BinaryServer test(8020,notify);
}
+namespace {
+void SaveImageToFile(IplImage *image, const char *filename) {
+ FILE *file = fopen(filename, "w");
+
+ fputs("P3\n320 240\n255\n", file);
+ ::cv::Mat img(image);
+ for (int i = 0; i < img.rows; i++) {
+ for (int j = 0; j < img.cols; j++) {
+ // You can now access the pixel value with cv::Vec3b
+ fprintf(file, "%d %d %d ",
+ img.at<cv::Vec3b>(i,j)[0],
+ img.at<cv::Vec3b>(i,j)[1],
+ img.at<cv::Vec3b>(i,j)[2]);
+
+ }
+ fputs("\n", file);
+ }
+ fclose(file);
+}
+}
+
#include "frc971/queues/CameraTarget.q.h"
using frc971::vision::targets;
@@ -48,59 +69,69 @@
ProcessorData processor(size.width, size.height, false);
printf("started sender main\n");
+ LOG(INFO, "Camera server started\n");
while(true){
//usleep(7500);
size_t data_size;
timeval timestamp_timeval;
const void *image = buffers.GetNext(
- true, &data_size, ×tamp_timeval, NULL);
- ::aos::time::Time timestamp(timestamp_timeval);
+ true, &data_size, ×tamp_timeval, NULL);
+ ::aos::time::Time timestamp(timestamp_timeval);
//TODO(pschuh): find proper way to cast away const for this: :(
- // parker you prolly want const_cast<type>(var);
- printf("\nNew Image Recieved\n");
- std::cout << timestamp << std::endl;
+ // parker you prolly want const_cast<type>(var);
+ LOG(INFO, "Got new image\n");
unsigned char *buffer = (unsigned char *)notify->GetBuffer();
- frc971::vision::process_jpeg(buffer, (unsigned char *)image, data_size);
+ frc971::vision::process_jpeg(buffer, (unsigned char *)image, data_size);
+
// build the headers on top of the buffer
- cvSetData(processor.src_header_image,
- buffer,
- ::aos::camera::Buffers::Buffers::kWidth * 3);
+ cvSetData(processor.src_header_image,
+ buffer,
+ ::aos::camera::Buffers::Buffers::kWidth * 3);
- // transform the buffer into targets
- processor.threshold(buffer);
- processor.getContours();
- processor.filterToTargets();
+ // Reset.
+ processor.clear();
+ // transform the buffer into targets
+ processor.threshold(buffer);
- // could be used for debug ie drawContours
- //std::vector<std::vector<cv::Point> > target_contours;
- //std::vector<std::vector<cv::Point> > best_contours;
- std::vector<Target>::iterator target_it;
- Target *best_target = NULL;
- // run through the targets
- for(target_it = processor.target_list.begin();
- target_it != processor.target_list.end(); target_it++){
- //target_contours.push_back(*(target_it->this_contour));
- // select the highest target
- if (best_target == NULL) {
- best_target = &*target_it;
- } else {
- if (target_it->height < best_target->height) {
- best_target = &*target_it;
- }
- }
- }
- // if we found one then send it on
- if (best_target != NULL) {
- targets.MakeWithBuilder()
- .percent_azimuth_off_center(
- best_target->rect.centroid.y / (double)::aos::camera::Buffers::kHeight - 0.5)
- .percent_elevation_off_center(
- best_target->rect.centroid.x / (double)::aos::camera::Buffers::kWidth - 0.5)
- .timestamp(timestamp.ToNSec())
- .Send();
+ processor.getContours();
+ processor.filterToTargets();
+
+ // could be used for debug ie drawContours
+ //std::vector<std::vector<cv::Point> > target_contours;
+ //std::vector<std::vector<cv::Point> > best_contours;
+ std::vector<Target>::iterator target_it;
+ Target *best_target = NULL;
+ // run through the targets
+ LOG(INFO, "Found %u targets\n", processor.target_list.size());
+ for(target_it = processor.target_list.begin();
+ target_it != processor.target_list.end(); target_it++){
+ //target_contours.push_back(*(target_it->this_contour));
+ // select the highest target
+ if (best_target == NULL) {
+ best_target = &*target_it;
+ } else {
+ if (target_it->height < best_target->height) {
+ best_target = &*target_it;
+ }
+ }
+ }
+ // if we found one then send it on
+ if (best_target != NULL) {
+ LOG(INFO, "Target is %f\n", best_target->rect.centroid.x);
+ targets.MakeWithBuilder()
+ .percent_azimuth_off_center(
+ best_target->rect.centroid.y / (double)::aos::camera::Buffers::kHeight - 0.5)
+ .percent_elevation_off_center(
+ best_target->rect.centroid.x / (double)::aos::camera::Buffers::kWidth - 0.5)
+ .timestamp(timestamp.ToNSec())
+ .Send();
}
notify->Notify();
+ //static int counter = 0;
+ //if (++counter > 2) {
+ //break;
+ //}
}
::aos::Cleanup();
}
diff --git a/vision/PacketNotifier.cpp b/vision/PacketNotifier.cpp
index f74be7f..d12872e 100644
--- a/vision/PacketNotifier.cpp
+++ b/vision/PacketNotifier.cpp
@@ -38,12 +38,12 @@
if(i != sending && i != to_send){ //open
filling = i;
mutex.Unlock();
- printf("leasing out to fill buff # %d\n",i);
+ //printf("leasing out to fill buff # %d\n",i);
return buffs[i];
}
}
mutex.Unlock();
- printf("Error in the fabric of the universe\n");
+ //printf("Error in the fabric of the universe\n");
exit(-42);
}
void PacketNotifier::Notify(){
@@ -56,7 +56,7 @@
}
void PacketNotifier::DataSent(const void * /*data*/, size_t /*datalen*/){
- printf("packet_sent: %d; fill: %d; to_send: %d \n",sending,filling,to_send);
+ //printf("packet_sent: %d; fill: %d; to_send: %d \n",sending,filling,to_send);
mutex.Lock();
sending = -1;
mutex.Unlock();
diff --git a/vision/SensorProcessor.h b/vision/SensorProcessor.h
index fe9f0ff..c219245 100644
--- a/vision/SensorProcessor.h
+++ b/vision/SensorProcessor.h
@@ -16,14 +16,21 @@
{216.75 / 320.0, 2.794},
};
+// Must be in reverse order in meters.
static const Interpolation kMetersToShooterSpeeds[] = {
- {10.0, 200.0},
- {5.0, 175.0},
+ {12.5, 375.0},
+ {21.0, 360.0},
+ {25.0, 375.0},
};
static const Interpolation kMetersToShooterAngles[] = {
- {10.0, 0.7},
- {5.0, 0.9},
+ {0.0, 0.7267},
+ {12.5, 0.7267},
+ {16.5, 0.604},
+ {18.0, 0.587},
+ {20.0, 0.576},
+ {21.0, 0.550},
+ {23.0, 0.540},
};
double interpolate(int num_interp_vals,