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; }