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);
}
int main( int argc, char** argv )
{
if (argc == 1)
{
std::cerr << "Usage: ./sift_detect -i image.pgm -o image.key "
<< "[-t warp.xfm -i2 warped.pgm -o2 warped.key] [options]\n"
<< " number of points to take: -p 800\n"
<< " scale upper bound 1: -ub1 6\n"
<< " scale lower bound 1: -lb1 2\n"
<< " scale upper bound 2: -ub2 9\n"
<< " scale lower bound 2: -lb2 2.8\n";
return 0;
}
char *image_file = NULL, *warped_file = NULL, *transform_file = NULL;
int num_points = 800;
std::string key_file = "source.key", warped_key_file = "warped.key";
float scale_ub1 = -1, scale_lb1 = -1, scale_ub2 = -1, scale_lb2 = -1;
int arg = 0;
while (++arg < argc)
{
if (! strcmp(argv[arg], "-i"))
image_file = argv[++arg];
if (! strcmp(argv[arg], "-i2"))
warped_file = argv[++arg];
if (! strcmp(argv[arg], "-t"))
transform_file = argv[++arg];
if (! strcmp(argv[arg], "-o"))
key_file = argv[++arg];
if (! strcmp(argv[arg], "-o2"))
warped_key_file = argv[++arg];
if (! strcmp(argv[arg], "-p"))
num_points = atoi(argv[++arg]);
if (! strcmp(argv[arg], "-ub1"))
scale_ub1 = atof(argv[++arg]);
if (! strcmp(argv[arg], "-lb1"))
scale_lb1 = atof(argv[++arg]);
if (! strcmp(argv[arg], "-ub2"))
scale_ub2 = atof(argv[++arg]);
if (! strcmp(argv[arg], "-lb2"))
scale_lb2 = atof(argv[++arg]);
}
assert(image_file);
assert(!warped_file || transform_file);
// Load the source image
Image im = ReadPGMFile(image_file);
int W = im->cols;
int H = im->rows;
// Find keypoints in source image
LoweKeypoint lowe_keypts = NULL;
{
Timer t("SIFT detector");
lowe_keypts = GetKeypoints(im);
}
std::vector<KeypointFl> keypts;
for (LoweKeypoint pt = lowe_keypts; pt != NULL; pt = pt->next)
{
keypts.push_back(KeypointFl(pt->col, pt->row, pt->scale, pt->strength));
}
if (scale_ub1 > 0)
keypts.erase(std::remove_if(keypts.begin(), keypts.end(),
ScaleUpperBound(scale_ub1)),
keypts.end());
if (scale_lb1 > 0)
keypts.erase(std::remove_if(keypts.begin(), keypts.end(),
ScaleLowerBound(scale_lb1)),
keypts.end());
Image warped = NULL;
CvMat *transform = NULL, *inv = NULL;
if (warped_file)
{
warped = ReadPGMFile(warped_file);
// Get inverse transform (warped -> source)
transform = cvCreateMat(3, 3, CV_32FC1);
ReadTransform(transform_file, transform);
inv = cvCreateMat(3, 3, CV_32FC1);
cvInvert(transform, inv);
keypts.erase(std::remove_if(keypts.begin(), keypts.end(),
OutsideSource(warped->cols, warped->rows, transform)),
keypts.end());
}
if ((int)keypts.size() < num_points)
{
num_points = keypts.size();
printf("WARNING: Only taking %d points!\n", num_points);
std::sort(keypts.begin(), keypts.end());
}
else
{
std::partial_sort(keypts.begin(), keypts.begin()+ num_points, keypts.end());
}
if (warped_file)
{
// Find keypoints in warped image
LoweKeypoint lowe_warp_keypts = NULL;
{
Timer t("SIFT detector (warped)");
lowe_warp_keypts = GetKeypoints(warped);
}
std::vector<KeypointFl> warp_keypts;
for (LoweKeypoint pt = lowe_warp_keypts; pt != NULL; pt = pt->next)
{
warp_keypts.push_back(KeypointFl(pt->col, pt->row, pt->scale, pt->strength));
}
if (scale_ub2 > 0)
warp_keypts.erase(std::remove_if(warp_keypts.begin(), warp_keypts.end(),
ScaleUpperBound(scale_ub2)),
warp_keypts.end());
if (scale_lb2 > 0)
warp_keypts.erase(std::remove_if(warp_keypts.begin(), warp_keypts.end(),
ScaleLowerBound(scale_lb2)),
warp_keypts.end());
warp_keypts.erase(std::remove_if(warp_keypts.begin(), warp_keypts.end(),
OutsideSource(W, H, inv)),
warp_keypts.end());
if ((int)warp_keypts.size() < num_points)
{
num_points = warp_keypts.size();
printf("WARNING: Only taking %d points!\n", num_points);
std::sort(warp_keypts.begin(), warp_keypts.end());
}
else
{
std::partial_sort(warp_keypts.begin(), warp_keypts.begin() + num_points,
warp_keypts.end());
warp_keypts.erase(warp_keypts.begin() + num_points, warp_keypts.end());
}
WriteKeypointsFl(warped_key_file, warp_keypts);
}
keypts.erase(keypts.begin() + num_points, keypts.end());
WriteKeypointsFl(key_file, keypts);
cvReleaseMat(&inv);
cvReleaseMat(&transform);
delete warped;
delete im;
return 0;
}
Status KeyframeVisualOdometry::Update(const Image& image) {
// Extract keypoints from the frame.
std::vector<Keypoint> keypoints;
Status status = GetKeypoints(image, &keypoints);
if (!status.ok()) return status;
// Extract features and descriptors from the keypoints.
std::vector<Feature> features;
std::vector<Descriptor> descriptors;
status = GetDescriptors(image, &keypoints, &features, &descriptors);
if (!status.ok()) return status;
// Set the annotator's image.
if (options_.draw_tracks || options_.draw_features) {
annotator_.SetImage(image);
}
// Initialize the very first view if we don't have one yet.
if (current_keyframe_ == kInvalidView) {
Landmark::SetRequiredObservations(2);
InitializeFirstView(features, descriptors);
return Status::Ok();
}
// Initialize the second view if we don't have one yet using 2D<-->2D
// matching.
if (view_indices_.size() == 1) {
status = InitializeSecondView(features, descriptors);
if (status.ok()) {
Landmark::SetRequiredObservations(options_.num_observations_to_triangulate);
}
return status;
}
// Create a camera with an unknown pose.
Camera camera;
camera.SetIntrinsics(intrinsics_);
View::Ptr new_view = View::Create(camera);
// Is this new image going to be a keyframe?
const bool is_keyframe =
initialize_new_keyframe_ ||
NumEstimatedTracks() < options_.min_num_feature_tracks;
if (is_keyframe) {
initialize_new_keyframe_ = false;
}
// Update feature tracks and add matched features to the view.
status = UpdateFeatureTracks(features,
descriptors,
new_view->Index(),
is_keyframe);
if (!status.ok()) {
View::DeleteMostRecentView();
return status;
}
// Compute the new camera pose.
status = EstimatePose(new_view->Index());
if (!status.ok()) {
View::DeleteMostRecentView();
} else {
view_indices_.push_back(new_view->Index());
}
if (is_keyframe && options_.perform_bundle_adjustment) {
// Bundle adjust views in the sliding window.
BundleAdjuster bundle_adjuster;
if (!bundle_adjuster.Solve(options_.bundle_adjustment_options,
SlidingWindowViewIndices()))
return Status::Cancelled("Failed to perform bundle adjustment.");
}
// Annotate tracks and features.
if (options_.draw_features) {
annotator_.AnnotateFeatures(features);
}
if (options_.draw_tracks) {
annotator_.AnnotateTracks(tracks_);
}
if (options_.draw_tracks || options_.draw_features) {
annotator_.Draw();
}
return status;
}
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;
}
