Ok. Here is the terminal output and attached are the files.
todd@todd-ubuntu:~/553/ceilingmosaics$ rosmake -s
[ rosmake ] No package specified. Building ['ceilingmosaics']
[ rosmake ] Packages requested are: ['ceilingmosaics']
[ rosmake ] Logging to directory
[ rosmake ] /home/todd/.ros/rosmake/rosmake_output-20100504-135534
[ rosmake ] Expanded args ['ceilingmosaics'] to:
['ceilingmosaics']
[ rosmake ] Checking rosdeps compliance for packages ceilingmosaics. This
may take a few seconds.
[ rosmake ] rosdep check passed all system dependencies in packages
[ rosmake ] specified-only option was used, only building packages
['ceilingmosaics']
[ rosmake ] Prebuilding rospack
[ rosmake ] Prebuilding gtest
[ rosmake ] Prebuilding genmsg_cpp
[ rosmake ] [ 0 of 1 Completed ]
[rosmake-0] >>> ceilingmosaics >>> [ make ]
[ rosmake ] Last 40 lines
{-------------------------------------------------------------------------------
[ 84%] make[3]: Leaving directory `/home/todd/553/ceilingmosaics/build'
Built target ROSBUILD_gensrv_java
[ 92%] make[3]: Entering directory `/home/todd/553/ceilingmosaics/build'
Built target ROSBUILD_gensrv_lisp
make[3]: Leaving directory `/home/todd/553/ceilingmosaics/build'
make[3]: Entering directory `/home/todd/553/ceilingmosaics/build'
[ 92%] Built target rospack_genmsg
make[3]: Entering directory `/home/todd/553/ceilingmosaics/build'
make[3]: Leaving directory `/home/todd/553/ceilingmosaics/build'
make[3]: Leaving directory `/home/todd/553/ceilingmosaics/build'
[ 92%] Built target rospack_genmsg_all
[ 92%] Built target rospack_gensrv
make[3]: Entering directory `/home/todd/553/ceilingmosaics/build'
make[3]: Entering directory `/home/todd/553/ceilingmosaics/build'
make[3]: Leaving directory `/home/todd/553/ceilingmosaics/build'
make[3]: Leaving directory `/home/todd/553/ceilingmosaics/build'
[ 92%] Built target rosbuild_precompile
[ 92%] make[3]: Entering directory `/home/todd/553/ceilingmosaics/build'
Built target rospack_gensrv_all
Scanning dependencies of target homography_generator
make[3]: Leaving directory `/home/todd/553/ceilingmosaics/build'
make[3]: Entering directory `/home/todd/553/ceilingmosaics/build'
[100%] Building CXX object
CMakeFiles/homography_generator.dir/src/homography_generator.o
/home/todd/553/ceilingmosaics/src/homography_generator.cpp: In function
‘void imageCallback(const sensor_msgs::ImageConstPtr&)’:
/home/todd/553/ceilingmosaics/src/homography_generator.cpp:271: warning:
unused variable ‘i’
Linking CXX executable ../bin/homography_generator
CMakeFiles/homography_generator.dir/src/homography_generator.o: In
function `main':
/home/todd/553/ceilingmosaics/src/homography_generator.cpp:317: undefined
reference to `CeilingNode::CeilingNode()'
CMakeFiles/homography_generator.dir/src/homography_generator.o: In
function `__gnu_cxx::new_allocator<CeilingNode>::construct(CeilingNode*,
CeilingNode const&)':
/usr/include/c++/4.4/ext/new_allocator.h:105: undefined reference to
`CeilingNode::CeilingNode(CeilingNode const&)'
CMakeFiles/homography_generator.dir/src/homography_generator.o: In
function `__gnu_cxx::new_allocator<CeilingNode>::destroy(CeilingNode*)':
/usr/include/c++/4.4/ext/new_allocator.h:115: undefined reference to
`CeilingNode::~CeilingNode()'
collect2: ld returned 1 exit status
make[3]: *** [../bin/homography_generator] Error 1
make[3]: Leaving directory `/home/todd/553/ceilingmosaics/build'
make[2]: *** [CMakeFiles/homography_generator.dir/all] Error 2
make[2]: Leaving directory `/home/todd/553/ceilingmosaics/build'
make[1]: *** [all] Error 2
make[1]: Leaving directory `/home/todd/553/ceilingmosaics/build'
-------------------------------------------------------------------------------}
[ rosmake ] Output from build of package ceilingmosaics written to:
[ rosmake ]
/home/todd/.ros/rosmake/rosmake_output-20100504-135534/ceilingmosaics/build_output.log
[rosmake-0] <<< ceilingmosaics <<< [FAIL] [ 5.52 seconds ]
[ rosmake ] Halting due to failure in package ceilingmosaics.
[ rosmake ] Waiting for other threads to complete.
[ rosmake ] Summary output to directory
[ rosmake ] /home/todd/.ros/rosmake/rosmake_output-20100504-135534
todd@todd-ubuntu:~/553/ceilingmosaics$
--
Todd Anderson
(919) 710-0546
Washington University in St. Louis Class of 2011
School of Engineering and Applied Sciences
#include "CeilingNode.h"
CeilingNode::CeilingNode(float x, float y, float heading, float[9] h)
:x(x),y(y),heading(heading), empty(false)
{
for(int i = 0; i< 9; ++i){
root_homography[i] = h[i];
}
}
CeilingNode::CeilingNode(float x, float y, float h)
:x(x),y(y),heading(h), empty(true)
{
for(int i = 0; i< 9; ++i){
root_homography[i] = 0.0;
}
}
CeilingNode::CeilingNode(const CeilingNode & sn)
:x(sn.x), y(sn.y), heading(sn.heading), empty(sn.empty)
{
for(int i = 0; i< 9; ++i){
root_homography[i] = sn.root_homography[i];
}
}
CeilingNode::~CeilingNode()
{
}
CeilingNode::CeilingNode()
{}
#ifndef CEILINGNODE_H
#define CEILINGNODE_H
using namespace std;
class CeilingNode{
private:
float root_homography[9];
//image! FILL IN LATER
public:
float x;
float y;
double heading;
bool empty;
CeilingNode();
CeilingNode(float, float, float, float[9]); //ADD IMAGE PARAMETER
CeilingNode(float, float, float);
CeilingNode(const CeilingNode&);
~CeilingNode();
float homography[9];
//image();
};
#endif
/*
* A Demo to OpenCV Implementation of SURF
* Further Information Refer to "SURF: Speed-Up Robust Feature"
* Author: Liu Liu
*
liuliu.1987+opencv@gmail.com
*/
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <ceilingmosaics/Homography.h>
#include <ceilingmosaics/PoseFromHomography.h>
#include <iostream>
#include <vector>
#include <cv_bridge/CvBridge.h>
#include <image_transport/image_transport.h>
#include "CeilingNode.h"
#include <std_msgs/Float64.h>
#include <list>
using namespace std;
using namespace ros;
// define whether to use approximate nearest-neighbor search
#define USE_FLANN
NodeHandle node;
Publisher h_publisher;
FILE *file;
IplImage *image = 0;
IplImage* object=0;
CvSeq *objectKeypoints = 0;
CvSeq *objectDescriptors = 0;
int global_count = 0;
list<CeilingNode> node_map;
bool lost;
double
compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
{
double total_cost = 0;
assert( length % 4 == 0 );
for( int i = 0; i < length; i += 4 )
{
double t0 = d1[i] - d2[i];
double t1 = d1[i+1] - d2[i+1];
double t2 = d1[i+2] - d2[i+2];
double t3 = d1[i+3] - d2[i+3];
total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3;
if( total_cost > best )
break;
}
return total_cost;
}
int
naiveNearestNeighbor( const float* vec, int laplacian,
const CvSeq* model_keypoints,
const CvSeq* model_descriptors )
{
int length = (int)(model_descriptors->elem_size/sizeof(float));
int i, neighbor = -1;
double d, dist1 = 1e6, dist2 = 1e6;
CvSeqReader reader, kreader;
cvStartReadSeq( model_keypoints, &kreader, 0 );
cvStartReadSeq( model_descriptors, &reader, 0 );
for( i = 0; i < model_descriptors->total; i++ )
{
const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
const float* mvec = (const float*)reader.ptr;
CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
if( laplacian != kp->laplacian )
continue;
d = compareSURFDescriptors( vec, mvec, dist2, length );
if( d < dist1 )
{
dist2 = dist1;
dist1 = d;
neighbor = i;
}
else if ( d < dist2 )
dist2 = d;
}
if ( dist1 < 0.6*dist2 )
return neighbor;
return -1;
}
void
findPairs( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,
const CvSeq* imageKeypoints, const CvSeq* imageDescriptors, vector<int>& ptpairs )
{
int i;
CvSeqReader reader, kreader;
cvStartReadSeq( objectKeypoints, &kreader );
cvStartReadSeq( objectDescriptors, &reader );
ptpairs.clear();
for( i = 0; i < objectDescriptors->total; i++ )
{
const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
const float* descriptor = (const float*)reader.ptr;
CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
int nearest_neighbor = naiveNearestNeighbor( descriptor, kp->laplacian, imageKeypoints, imageDescriptors );
if( nearest_neighbor >= 0 )
{
ptpairs.push_back(i);
ptpairs.push_back(nearest_neighbor);
}
}
}
void
flannFindPairs( const CvSeq*, const CvSeq* objectDescriptors,
const CvSeq*, const CvSeq* imageDescriptors, vector<int>& ptpairs )
{
int length = (int)(objectDescriptors->elem_size/sizeof(float));
cv::Mat m_object(objectDescriptors->total, length, CV_32F);
cv::Mat m_image(imageDescriptors->total, length, CV_32F);
// copy descriptors
CvSeqReader obj_reader;
float* obj_ptr = m_object.ptr<float>(0);
cvStartReadSeq( objectDescriptors, &obj_reader );
for(int i = 0; i < objectDescriptors->total; i++ )
{
const float* descriptor = (const float*)obj_reader.ptr;
CV_NEXT_SEQ_ELEM( obj_reader.seq->elem_size, obj_reader );
memcpy(obj_ptr, descriptor, length*sizeof(float));
obj_ptr += length;
}
CvSeqReader img_reader;
float* img_ptr = m_image.ptr<float>(0);
cvStartReadSeq( imageDescriptors, &img_reader );
for(int i = 0; i < imageDescriptors->total; i++ )
{
const float* descriptor = (const float*)img_reader.ptr;
CV_NEXT_SEQ_ELEM( img_reader.seq->elem_size, img_reader );
memcpy(img_ptr, descriptor, length*sizeof(float));
img_ptr += length;
}
// find nearest neighbors using FLANN
cv::Mat m_indices(objectDescriptors->total, 2, CV_32S);
cv::Mat m_dists(objectDescriptors->total, 2, CV_32F);
cv::flann::Index flann_index(m_image, cv::flann::KDTreeIndexParams(4)); // using 4 randomized kdtrees
flann_index.knnSearch(m_object, m_indices, m_dists, 2, cv::flann::SearchParams(64) ); // maximum number of leafs checked
int* indices_ptr = m_indices.ptr<int>(0);
float* dists_ptr = m_dists.ptr<float>(0);
for (int i=0;i<m_indices.rows;++i) {
if (dists_ptr[2*i]<0.6*dists_ptr[2*i+1]) {
ptpairs.push_back(i);
ptpairs.push_back(indices_ptr[2*i]);
}
}
}
/* a rough implementation for object location */
int locatePlanarObject( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors,
const CvSeq* imageKeypoints, const CvSeq* imageDescriptors,
const CvPoint src_corners[4], CvPoint dst_corners[4], ros::Time stamp )
{
double h[9];
CvMat _h = cvMat(3, 3, CV_64F, h);
vector<int> ptpairs;
vector<CvPoint2D32f> pt1, pt2;
CvMat _pt1, _pt2;
int i, n;
#ifdef USE_FLANN
flannFindPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
#else
findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
#endif
n = ptpairs.size()/2;
if( n < 4 )
return 0;
pt1.resize(n);
pt2.resize(n);
for( i = 0; i < n; i++ )
{
pt1[i] = ((CvSURFPoint*)cvGetSeqElem(objectKeypoints,ptpairs[i*2]))->pt;
pt2[i] = ((CvSURFPoint*)cvGetSeqElem(imageKeypoints,ptpairs[i*2+1]))->pt;
}
_pt1 = cvMat(1, n, CV_32FC2, &pt1[0] );
_pt2 = cvMat(1, n, CV_32FC2, &pt2[0] );
if( !cvFindHomography( &_pt1, &_pt2, &_h, CV_RANSAC, 0.1 ))
return 0;
ceilingmosaics::Homography h_m;
++global_count;
if(n>3){
int count = 0;
CvMat _svd = cvMat(3, 3, CV_64F, h);
cvSVD(&_h, &_svd, 0, 0, 0);
for(int j=0; j<3;++j){
for(int k=0; k<3; ++k){
if(cvmGet(&_svd, j, k)>.01)
++count;
}
}
if(count==3){
cvFindHomography( &_pt1, &_pt2, &_h, CV_RANSAC, 5 );
for(int j=0; j<3;++j){
for(int k=0; k<3; ++k){
h_m.mat[j+k*3] = cvmGet(&_h, j, k);
}
}
h_m.header.stamp = stamp;
ros::ServiceClient client = node.serviceClient<ceilingmosaics::PoseFromHomography>("pose_from_homography");
ceilingmosaics::PoseFromHomography srv;
srv.request.H = h_m;
if (client.call(srv))
{
ROS_ERROR("Pose: %f", (float)srv.response.pose.x);
}
else
{
ROS_ERROR("Failed to call service add_two_ints");
}
h_publisher.publish(h_m);
}
else
lost=false;
}
else{
//ROS_ERROR("REJECT N: %i",n);
lost = false;
}
for( i = 0; i < 4; i++ )
{
double x = src_corners[i].x, y = src_corners[i].y;
double Z = 1./(h[6]*x + h[7]*y + h[8]);
double X = (h[0]*x + h[1]*y + h[2])*Z;
double Y = (h[3]*x + h[4]*y + h[5])*Z;
dst_corners[i] = cvPoint(cvRound(X), cvRound(Y));
}
return 1;
}
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
sensor_msgs::CvBridge bridge;
lost =true;
try
{
CvMemStorage* storage = cvCreateMemStorage(0);
IplImage* image = bridge.imgMsgToCv(msg, "mono8");
// cv::imshow("current image", image); cv::waitKey(10);
CvSeq *imageKeypoints = 0, *imageDescriptors = 0;
int i;
CvSURFParams params = cvSURFParams(500, 1);
cvExtractSURF( image, 0, &imageKeypoints, &imageDescriptors, storage, params );
if(object==0 && lost){
objectKeypoints = imageKeypoints;
objectDescriptors = imageDescriptors;
object = image;
return;
}
CvPoint src_corners[4] = {{0,0}, {object->width,0}, {object->width, object->height}, {0, object->height}};
CvPoint dst_corners[4];
if( locatePlanarObject( objectKeypoints, objectDescriptors, imageKeypoints,
imageDescriptors, src_corners, dst_corners, msg->header.stamp ))
{
//do nothing
}
/*vector<int> ptpairs;
#ifdef USE_FLANN
flannFindPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
#else
findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
#endif
*/
if(lost){
objectKeypoints = imageKeypoints;
objectDescriptors = imageDescriptors;
object = image;
}
}
catch (sensor_msgs::CvBridgeException& e)
{
ROS_ERROR("Could not convert from '%s' to 'mono8'.", msg->encoding.c_str());
}
}
int main(int argc, char** argv)
{
init(argc, argv, "surf");
file = fopen("homographies.txt","rw");
cv::namedWindow("current image");
h_publisher = node.advertise<ceilingmosaics::Homography>("homography",1);
image_transport::ImageTransport it(node);
image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback);
CeilingNode * start = new CeilingNode();
node_map.push_back(*start);
ros::spin();
//fclose(file);
return 0;
}
(text/plain)