Revision 91a0bfa9
| ID | 91a0bfa9fd4c7c4b078b623fab8a9da5c7558195 |
Commented code!
| vision/src/v2v3_converter.cpp | ||
|---|---|---|
| 1 |
#include <ros/ros.h>
|
|
| 1 |
#nclude <ros/ros.h> |
|
| 2 | 2 |
#include <stdio.h> |
| 3 | 3 |
#include <vision/TargetDescriptor.h> |
| 4 | 4 |
#include <image_transport/image_transport.h> |
| 5 | 5 |
#include <geometry_msgs/Point.h> |
| 6 | 6 |
#include <tf/transform_listener.h> |
| 7 | 7 |
|
| 8 |
ros::Publisher pub; |
|
| 9 |
ros::Publisher img_pub; |
|
| 8 |
ros::Publisher pub; // publishes Point
|
|
| 9 |
ros::Publisher img_pub; //publishes Image with only blob showing
|
|
| 10 | 10 |
|
| 11 |
int hk = 15; |
|
| 11 |
int hk = 15; // target hsv values
|
|
| 12 | 12 |
int sk = 255; |
| 13 | 13 |
int vk = 255; |
| 14 | 14 |
|
| 15 |
/* Given index p and image pointer img, |
|
| 16 |
* determines whether or not the pixel |
|
| 17 |
* in the indicated vicinity is equal to it. |
|
| 18 |
*/ |
|
| 15 | 19 |
bool isRight(sensor_msgs::Image * img, int p){
|
| 16 | 20 |
return (img->data[p + 1] == img->data[p]); |
| 17 | 21 |
} |
| ... | ... | |
| 28 | 32 |
return (img->data[p + img->width] == img->data[p]); |
| 29 | 33 |
} |
| 30 | 34 |
|
| 35 |
/* Checks for any adjacent pixel, ensuring a continuous blob. |
|
| 36 |
*/ |
|
| 31 | 37 |
bool hasAnAdjacent(sensor_msgs::Image * image, int j){
|
| 32 | 38 |
int nl = image->height; // number of lines |
| 33 | 39 |
int nc = image->width; |
| ... | ... | |
| 54 | 60 |
void detectBlobs(sensor_msgs::Image * image, int * comX, int * comY, int * area){
|
| 55 | 61 |
int nl = image->height; // number of lines |
| 56 | 62 |
int nc = image->step; // number of columns |
| 57 |
int nChannels = image->step / image->width; // effective width |
|
| 63 |
int nChannels = image->step / image->width; // effective width of each pixel
|
|
| 58 | 64 |
|
| 59 | 65 |
// process image |
| 60 | 66 |
for (int i = 0; i < nl; i++){
|
| 61 | 67 |
for (int j = 0; j < nc; j+= nChannels){
|
| 62 |
if (image->data[i * image->width + j] <= hk + 10 && image->data[j] >= hk - 10){
|
|
| 63 |
image->data[i * image->width + j] = 255; |
|
| 68 |
// if within bounds... |
|
| 69 |
if (image->data[i * image->width + j] <= hk + 10 && image->data[j] >= hk - 10){
|
|
| 70 |
image->data[i * image->width + j] = 255; // set each to 255 |
|
| 64 | 71 |
image->data[i * image->width + j+1] = 255; |
| 65 | 72 |
image->data[i * image->width + j+2] = 255; |
| 66 | 73 |
} |
| 74 |
// otherwise set to 0 |
|
| 67 | 75 |
else {
|
| 68 | 76 |
image->data[i * image->width + j] = 0; |
| 69 | 77 |
image->data[i * image->width + j+1] = 0; |
| ... | ... | |
| 72 | 80 |
} |
| 73 | 81 |
} |
| 74 | 82 |
|
| 75 |
int comXSum = 0; |
|
| 76 |
int comYSum = 0; |
|
| 77 |
int blobPixelCount = 0; |
|
| 83 |
|
|
| 84 |
// The following is to find the average |
|
| 85 |
int comXSum = 0; |
|
| 86 |
int comYSum = 0; |
|
| 87 |
int blobPixelCount = 0; |
|
| 78 | 88 |
for (int i = 0; i < image->width * image->height; i+=nChannels){
|
| 89 |
// if the pixel is not both the right hue and has adjacents, set to 0 |
|
| 79 | 90 |
if ( !(image->data[i] == 255 && hasAnAdjacent(image, i)) ){
|
| 80 | 91 |
image->data[i] = 0; |
| 81 | 92 |
image->data[i + 1] = 0; |
| 82 | 93 |
image->data[i + 2] = 0; |
| 83 |
continue; |
|
| 84 | 94 |
} |
| 85 |
blobPixelCount++; |
|
| 86 |
comXSum += image->data[i] / 255; |
|
| 87 |
comYSum += image->data[i] / 255; |
|
| 95 |
else {
|
|
| 96 |
blobPixelCount++; |
|
| 97 |
comXSum += image->data[i] / 255; |
|
| 98 |
comYSum += image->data[i] / 255; |
|
| 99 |
} |
|
| 88 | 100 |
} |
| 89 | 101 |
if (blobPixelCount != 0){
|
| 90 | 102 |
// get average |
| 91 | 103 |
comXSum /= blobPixelCount; |
| 92 | 104 |
comYSum /= blobPixelCount; |
| 93 | 105 |
} |
| 94 |
|
|
| 106 |
// output results |
|
| 95 | 107 |
*area = blobPixelCount; |
| 96 |
*comX = comXSum;
|
|
| 97 |
*comY = comYSum;
|
|
| 108 |
*comX = comXSum;
|
|
| 109 |
*comY = comYSum;
|
|
| 98 | 110 |
|
| 99 |
img_pub.publish(*image);
|
|
| 111 |
img_pub.publish(*image); // publish new image of only black and white
|
|
| 100 | 112 |
} |
| 101 | 113 |
|
| 102 | 114 |
void convertToHSV(sensor_msgs::Image * orig){
|
| ... | ... | |
| 113 | 125 |
int min; |
| 114 | 126 |
|
| 115 | 127 |
// get min, max |
| 116 |
if (orig->data[i] >= orig->data[i+1] && orig->data[i] >= orig->data[i+2]){
|
|
| 117 |
v = orig->data[i]; |
|
| 118 |
if (orig->data[i+1] >= orig->data[i+2]){
|
|
| 119 |
min = orig->data[i+2]; |
|
| 120 |
} |
|
| 121 |
else min = orig->data[i+1]; |
|
| 122 |
} |
|
| 123 |
else if (orig->data[i+1] >= orig->data[i] && orig->data[i+1] >= orig->data[i+2]){
|
|
| 124 |
v = orig->data[i+1]; |
|
| 125 |
if (orig->data[i] >= orig->data[i+2]){
|
|
| 126 |
min = orig->data[i+2]; |
|
| 127 |
} |
|
| 128 |
else min = orig->data[i]; |
|
| 129 |
} |
|
| 130 |
else {
|
|
| 131 |
v = orig->data[i+2]; |
|
| 132 |
if (orig->data[i] >= orig->data[i+1]){
|
|
| 133 |
min = orig->data[i+1]; |
|
| 134 |
} |
|
| 135 |
else min = orig->data[i]; |
|
| 136 |
} |
|
| 128 |
v = max(max(orig->data[i], orig->data[i+1]), orig->data[i+2]); |
|
| 129 |
min = min(min(orig->data[i], orig->data[i+1]), orig->data[i+2]); |
|
| 137 | 130 |
|
| 138 | 131 |
// set s |
| 139 | 132 |
if (v != 0){
|
| ... | ... | |
| 153 | 146 |
h = 240 + 60 * (orig->data[i] - orig->data[i + 1]) / s; |
| 154 | 147 |
} |
| 155 | 148 |
|
| 149 |
// set values |
|
| 156 | 150 |
orig->data[i] = h; |
| 157 | 151 |
orig->data[i+1] = s; |
| 158 | 152 |
orig->data[i+2] = v; |
| ... | ... | |
| 174 | 168 |
ROS_ERROR("%s", ex.what());
|
| 175 | 169 |
}*/ |
| 176 | 170 |
|
| 177 |
// convert to OpenCV
|
|
| 171 |
// image types
|
|
| 178 | 172 |
|
| 179 | 173 |
sensor_msgs::Image copy = orig; |
| 180 | 174 |
sensor_msgs::Image * source = © // pointer to the image |
| ... | ... | |
| 196 | 190 |
int distance; |
| 197 | 191 |
|
| 198 | 192 |
// give distance as inversely proportional to area, for (x,y,z) |
| 199 |
if (area > 10){
|
|
| 193 |
if (area > 1){
|
|
| 200 | 194 |
|
| 201 | 195 |
distance = 100.0 / (float) area; |
| 202 | 196 |
|
Also available in: Unified diff