int main() {
IplImage* lena = cvLoadImage("lena.tif", 0);
Image img_sift = CreateImage(lena->height, lena->width);
for(int r = 0; r < lena->height; ++r)
{
for(int c = 0; c < lena->width; ++c)
{
CvScalar s = cvGet2D(lena,c,r);
img_sift->pixels[r*img_sift->stride+c] =
(s.val[0] + s.val[1] + s.val[2]) / (3.*255);
}
}
Keypoint keypts = GetKeypoints(img_sift);
Keypoint key = keypts;
while(key)
{
cvRectangle(lena,
cvPoint(key->row-1 ,key->col-1),
cvPoint(key->row+1 ,key->col+1),
cvScalar(255,0,0), 1);
key = key->next;
}
FreeKeypoints(keypts);
DestroyAllResources();
cvNamedWindow("lena", 1);
cvMoveWindow("lena", 800, 200);
cvShowImage("lena", lena);
cvWaitKey(0);
cvDestroyAllWindows();
cvReleaseImage(&lena);
}
bool SiftNode::detect(posedetection_msgs::Feature0D& features, const sensor_msgs::Image& imagemsg,
const sensor_msgs::Image::ConstPtr& mask_ptr)
{
boost::mutex::scoped_lock lock(_mutex);
Image imagesift = NULL;
cv::Rect region;
try {
cv::Mat image;
cv_bridge::CvImagePtr framefloat;
if (!(framefloat = cv_bridge::toCvCopy(imagemsg, "mono8")) )
return false;
if(imagesift != NULL && (imagesift->cols!=imagemsg.width || imagesift->rows!=imagemsg.height)) {
ROS_INFO("clear sift resources");
DestroyAllImages();
imagesift = NULL;
}
image = framefloat->image;
if (mask_ptr) {
cv::Mat mask = cv_bridge::toCvShare(mask_ptr, mask_ptr->encoding)->image;
region = jsk_perception::boundingRectOfMaskImage(mask);
image = image(region);
}
else {
region = cv::Rect(0, 0, imagemsg.width, imagemsg.height);
}
if(imagesift == NULL)
imagesift = CreateImage(imagemsg.height,imagemsg.width);
for(int i = 0; i < imagemsg.height; ++i) {
uint8_t* psrc = (uint8_t*)image.data+image.step*i;
float* pdst = imagesift->pixels+i*imagesift->stride;
for(int j = 0; j < imagemsg.width; ++j)
pdst[j] = (float)psrc[j]*(1.0f/255.0f);
//memcpy(imagesift->pixels+i*imagesift->stride,framefloat->imageData+framefloat->widthStep*i,imagemsg.width*sizeof(float));
}
}
catch (cv_bridge::Exception error) {
ROS_WARN("bad frame");
return false;
}
// compute SIFT
ros::Time siftbasetime = ros::Time::now();
Keypoint keypts = GetKeypoints(imagesift);
// write the keys to the output
int numkeys = 0;
Keypoint key = keypts;
while(key) {
numkeys++;
key = key->next;
}
// publish
features.header = imagemsg.header;
features.positions.resize(numkeys*2);
features.scales.resize(numkeys);
features.orientations.resize(numkeys);
features.confidences.resize(numkeys);
features.descriptors.resize(numkeys*128);
features.descriptor_dim = 128;
features.type = "libsiftfast";
int index = 0;
key = keypts;
while(key) {
for(int j = 0; j < 128; ++j)
features.descriptors[128*index+j] = key->descrip[j];
features.positions[2*index+0] = key->col + region.x;
features.positions[2*index+1] = key->row + region.y;
features.scales[index] = key->scale;
features.orientations[index] = key->ori;
features.confidences[index] = 1.0; // SIFT has no confidence?
key = key->next;
++index;
}
FreeKeypoints(keypts);
DestroyAllImages();
ROS_INFO("imagesift: image: %d(size=%lu), num: %d, sift time: %.3fs, total: %.3fs", imagemsg.header.seq,
imagemsg.data.size(), numkeys,
(float)(ros::Time::now()-siftbasetime).toSec(), (float)(ros::Time::now()-lasttime).toSec());
lasttime = ros::Time::now();
return true;
}
