SamplePointcloud.cpp
Go to the documentation of this file.
1 #include "CvTestbed.h"
2 #include "GlutViewer.h"
3 #include "SfM.h"
4 #include "Shared.h"
5 using namespace alvar;
6 using namespace std;
7 
8 const double marker_size=1;
9 bool init=true;
11 std::stringstream calibrationFilename;
12 
13 // Own drawable 3D cross on features
14 struct OwnDrawable : public Drawable {
15  virtual void Draw() {
16  const double scale = 0.2;
17  glPushMatrix();
18  glMultMatrixd(gl_mat);
19  glColor3d(color[0], color[1], color[2]);
20  glBegin(GL_LINES);
21  glVertex3f(0.0, 0.0, -scale);
22  glVertex3f(0.0, 0.0, scale);
23  glVertex3f(0.0, -scale, 0.0);
24  glVertex3f(0.0, scale, 0.0);
25  glVertex3f(-scale, 0.0, 0.0);
26  glVertex3f(scale, 0.0, 0.0);
27  glEnd();
28  glPopMatrix();
29  }
30 };
34 
35 bool reset=false;
36 void videocallback(IplImage *image)
37 {
38  static IplImage *rgb = 0;
39  static IplImage* bg_image = 0;
40 
41  bool flip_image = (image->origin?true:false);
42  if (flip_image) {
43  cvFlip(image);
44  image->origin = !image->origin;
45  }
46 
47  if (init) {
48  init = false;
49  sfm->Clear();
50  cout<<"Loading calibration: "<<calibrationFilename.str();
51  if (sfm->GetCamera()->SetCalib(calibrationFilename.str().c_str(), image->width, image->height)) {
52  cout<<" [Ok]"<<endl;
53  } else {
54  sfm->GetCamera()->SetRes(image->width, image->height);
55  cout<<" [Fail]"<<endl;
56  }
57  double p[16];
58  sfm->GetCamera()->GetOpenglProjectionMatrix(p,image->width,image->height);
60  d_marker.SetScale(marker_size*2);
61  rgb = CvTestbed::Instance().CreateImageWithProto("RGB", image, 0, 3);
63  bg_image = CvTestbed::Instance().CreateImage("BG texture", cvSize(512,512),8, 3);
64  bg_image->origin = 0;
65 
66  sfm->SetScale(10);
67  if (sfm->AddMultiMarker("mmarker.xml", FILE_FORMAT_XML)) {
68  std::cout<<"Using MultiMarker defined in mmarker.xml."<<std::endl;
69  } else {
70  std::cout<<"Couldn't load mmarker.xml. Using default 'SampleMultiMarker' setup."<<std::endl;
71  Pose pose;
72  pose.Reset();
73  sfm->AddMarker(0, marker_size*2, pose);
74  pose.SetTranslation(-marker_size*2.5, +marker_size*1.5, 0);
75  sfm->AddMarker(1, marker_size, pose);
76  pose.SetTranslation(+marker_size*2.5, +marker_size*1.5, 0);
77  sfm->AddMarker(2, marker_size, pose);
78  pose.SetTranslation(-marker_size*2.5, -marker_size*1.5, 0);
79  sfm->AddMarker(3, marker_size, pose);
80  pose.SetTranslation(+marker_size*2.5, -marker_size*1.5, 0);
81  sfm->AddMarker(4, marker_size, pose);
82  }
83  sfm->SetResetPoint();
84  }
85  if (reset) {
86  sfm->Reset();
87  reset = false;
88  }
89 
90  //if (sfm->UpdateRotationsOnly(image)) {
91  //if (sfm->UpdateTriangulateOnly(image)) {
92  if (sfm->Update(image, false, true, 7.f, 15.f)) {
93  // Draw the camera (The GlutViewer has little weirdness here...)q
94  Pose pose = *(sfm->GetPose());
95  double gl[16];
96  pose.GetMatrixGL(gl, true);
98  pose.Invert();
99  pose.GetMatrixGL(d_marker.gl_mat, false);
101  GlutViewer::DrawableAdd(&d_marker);
103 
104  // Draw features
105  std::map<int, SimpleSfM::Feature>::iterator iter;
106  iter = sfm->container.begin();
107  for(;iter != sfm->container.end(); iter++) {
108  if (sfm->container_triangulated.find(iter->first) != sfm->container_triangulated.end()) continue;
109  if (iter->second.has_p3d)
110  {
111  if (own_drawable_count < 1000) {
112  memset(d_points[own_drawable_count].gl_mat, 0, 16*sizeof(double));
113  d_points[own_drawable_count].gl_mat[0] = 1;
114  d_points[own_drawable_count].gl_mat[5] = 1;
115  d_points[own_drawable_count].gl_mat[10] = 1;
116  d_points[own_drawable_count].gl_mat[15] = 1;
117  d_points[own_drawable_count].gl_mat[12] = iter->second.p3d.x;
118  d_points[own_drawable_count].gl_mat[13] = iter->second.p3d.y;
119  d_points[own_drawable_count].gl_mat[14] = iter->second.p3d.z;
120  if (iter->second.type_id == 0) d_points[own_drawable_count].SetColor(1,0,0);
121  else d_points[own_drawable_count].SetColor(0,1,0);
123  own_drawable_count++;
124  }
125  }
126  }
127 
128  // Draw triangulated features
129  iter = sfm->container_triangulated.begin();
130  for(;iter != sfm->container_triangulated.end(); iter++) {
131  if (iter->second.has_p3d)
132  {
133  if (own_drawable_count < 1000) {
134  memset(d_points[own_drawable_count].gl_mat, 0, 16*sizeof(double));
135  d_points[own_drawable_count].gl_mat[0] = 1;
136  d_points[own_drawable_count].gl_mat[5] = 1;
137  d_points[own_drawable_count].gl_mat[10] = 1;
138  d_points[own_drawable_count].gl_mat[15] = 1;
139  d_points[own_drawable_count].gl_mat[12] = iter->second.p3d.x;
140  d_points[own_drawable_count].gl_mat[13] = iter->second.p3d.y;
141  d_points[own_drawable_count].gl_mat[14] = iter->second.p3d.z;
142  /*if (iter->second.type_id == 0) d_points[own_drawable_count].SetColor(1,0,1);
143  else*/ d_points[own_drawable_count].SetColor(0,0,1);
145  own_drawable_count++;
146  }
147  }
148  }
149  }
150  if (image->nChannels == 1) cvCvtColor(image, rgb, CV_GRAY2RGB);
151  else if (image->nChannels == 3) cvCopy(image, rgb);
152 
153  // Draw video on GlutViewer background
154  cvResize(rgb, bg_image);
155  GlutViewer::SetVideo(bg_image);
156 
157  // Draw debug info to the rgb
158  sfm->Draw(rgb);
159 
160  if (flip_image) {
161  cvFlip(image);
162  image->origin = !image->origin;
163  }
164 }
165 
166 
167 int keycallback(int key)
168 {
169  if(key == 'r')
170  {
171  reset = true;
172  }
173  else return key;
174 
175  return 0;
176 }
177 
178 int main(int argc, char *argv[])
179 {
180  try {
181  // Output usage message
182  std::string filename(argv[0]);
183  filename = filename.substr(filename.find_last_of('\\') + 1);
184  std::cout << "SamplePointcloud" << std::endl;
185  std::cout << "================" << std::endl;
186  std::cout << std::endl;
187  std::cout << "Description:" << std::endl;
188  std::cout << " This example shows simple structure from motion approach that can be "<< std::endl;
189  std::cout << " used to track environment beyond an multimarker setup. To get this "<< std::endl;
190  std::cout << " example work properly be sure to calibrate your camera and tune it "<< std::endl;
191  std::cout << " to have fast framerate without motion blur. "<< std::endl;
192  std::cout << std::endl;
193  std::cout << " There are two possible approaches Update() and UpdateRotationsOnly()."<< std::endl;
194  std::cout << " By default the Update() is used but you can easily uncomment the "<< std::endl;
195  std::cout << " other one if needed."<< std::endl;
196  std::cout << std::endl;
197  std::cout << "Usage:" << std::endl;
198  std::cout << " " << filename << " [device]" << std::endl;
199  std::cout << std::endl;
200  std::cout << " device integer selecting device from enumeration list (default 0)" << std::endl;
201  std::cout << " highgui capture devices are prefered" << std::endl;
202  std::cout << std::endl;
203  std::cout << "Keyboard Shortcuts:" << std::endl;
204  std::cout << " r: reset" << std::endl;
205  std::cout << " q: quit" << std::endl;
206  std::cout << std::endl;
207 
208  // Initialise GlutViewer and CvTestbed
209  GlutViewer::Start(argc, argv, 640, 480, 100);
212  sfm = new SimpleSfM();
213 
214  // Enumerate possible capture plugins
216  if (plugins.size() < 1) {
217  std::cout << "Could not find any capture plugins." << std::endl;
218  return 0;
219  }
220 
221  // Display capture plugins
222  std::cout << "Available Plugins: ";
223  outputEnumeratedPlugins(plugins);
224  std::cout << std::endl;
225 
226  // Enumerate possible capture devices
228  if (devices.size() < 1) {
229  std::cout << "Could not find any capture devices." << std::endl;
230  return 0;
231  }
232 
233  // Check command line argument for which device to use
234  int selectedDevice = defaultDevice(devices);
235  if (argc > 1) {
236  selectedDevice = atoi(argv[1]);
237  }
238  if (selectedDevice >= (int)devices.size()) {
239  selectedDevice = defaultDevice(devices);
240  }
241 
242  // Display capture devices
243  std::cout << "Enumerated Capture Devices:" << std::endl;
244  outputEnumeratedDevices(devices, selectedDevice);
245  std::cout << std::endl;
246 
247  // Create capture object from camera
248  Capture *cap = CaptureFactory::instance()->createCapture(devices[selectedDevice]);
249  std::string uniqueName = devices[selectedDevice].uniqueName();
250 
251  // Handle capture lifecycle and start video capture
252  // Note that loadSettings/saveSettings are not supported by all plugins
253  if (cap) {
254  std::stringstream settingsFilename;
255  settingsFilename << "camera_settings_" << uniqueName << ".xml";
256  calibrationFilename << "camera_calibration_" << uniqueName << ".xml";
257 
258  cap->start();
259  cap->setResolution(640, 480);
260 
261  if (cap->loadSettings(settingsFilename.str())) {
262  std::cout << "Loading settings: " << settingsFilename.str() << std::endl;
263  }
264 
265  std::stringstream title;
266  title << "SamplePointcloud (" << cap->captureDevice().captureType() << ")";
267 
268  CvTestbed::Instance().StartVideo(cap, title.str().c_str());
269 
270  if (cap->saveSettings(settingsFilename.str())) {
271  std::cout << "Saving settings: " << settingsFilename.str() << std::endl;
272  }
273 
274  cap->stop();
275  delete cap; cap = NULL;
276  }
277  else if (CvTestbed::Instance().StartVideo(0, argv[0])) {
278  }
279  else {
280  std::cout << "Could not initialize the selected capture backend." << std::endl;
281  }
282  delete sfm; sfm = NULL;
283 
284  return 0;
285  }
286  catch (const std::exception &e) {
287  std::cout << "Exception: " << e.what() << endl;
288  }
289  catch (...) {
290  std::cout << "Exception: unknown" << std::endl;
291  }
292 }
Main ALVAR namespace.
Definition: Alvar.h:174
int own_drawable_count
void GetOpenglProjectionMatrix(double proj_matrix[16], const int width, const int height, const float far_clip=1000.0f, const float near_clip=0.1f)
Get OpenGL matrix Generates the OpenGL projection matrix based on OpenCV intrinsic camera matrix K...
Definition: Camera.cpp:394
static CvTestbed & Instance()
The one and only instance of CvTestbed is accessed using CvTestbed::Instance()
Definition: CvTestbed.cpp:88
filename
Capture * createCapture(const CaptureDevice captureDevice)
Create Capture class. Transfers onwership to the caller.
void AddMarker(int marker_id, double edge_length, Pose &pose)
Add an marker to be a basis for tracking. It is good idea to call SetResetPoint after these...
Definition: SfM.cpp:98
const double marker_size
virtual void stop()=0
Stops the camera capture.
void SetScale(double s)
Set the suitable scale to be used. This affects quite much how the tracking behaves (when features ar...
Definition: SfM.h:101
void GetMatrixGL(double gl[16], bool mirror=true)
Get the transformation matrix representation of the Pose using OpenGL&#39;s transposed format...
Definition: Pose.cpp:104
virtual bool start()=0
Starts the camera capture.
Simple structure from motion implementation using CameraEC , MarkerDetectorEC and TrackerFeaturesEC...
Definition: SfM.h:41
bool init
void Clear()
Clear all tracked features.
Definition: SfM.cpp:81
void SetResetPoint()
Remember the current state and return here when the Reset is called.
Definition: SfM.cpp:76
Drawable d_marker
CaptureDevice captureDevice()
The camera information associated to this capture object.
Definition: Capture.h:70
void SetVideoCallback(void(*_videocallback)(IplImage *image))
Set the videocallback function that will be called for every frame.
Definition: CvTestbed.cpp:93
This file implements structure from motion.
static CaptureFactory * instance()
The singleton instance of CaptureFactory.
IplImage * CreateImage(const char *title, CvSize size, int depth, int channels)
Creates an image with given size, depth and channels and stores it with a given &#39;title&#39; (see CvTestbe...
Definition: CvTestbed.cpp:143
std::map< int, Feature > container_triangulated
Definition: SfM.h:72
IplImage * CreateImageWithProto(const char *title, IplImage *proto, int depth=0, int channels=0)
Creates an image based on the given prototype and stores it with a given &#39;title&#39; (see CvTestbed::SetI...
Definition: CvTestbed.cpp:150
void SetRes(int _x_res, int _y_res)
If we have no calibration file we can still adjust the default calibration to current resolution...
Definition: Camera.cpp:374
unsigned char * image
Definition: GlutViewer.cpp:155
double gl_mat[16]
Definition: GlutViewer.h:33
bool AddMultiMarker(const char *fname, FILE_FORMAT format=FILE_FORMAT_XML)
Add MultiMarker from file as a basis for tracking. It is good idea to call SetResetPoint after these...
Definition: SfM.cpp:89
CameraEC * GetCamera()
Get the camera used internally. You need to use this to set correct camera calibration (see SamplePoi...
Definition: SfM.cpp:85
std::vector< CaptureDevice > CaptureDeviceVector
Vector of CaptureDevices.
std::string captureType() const
The type of capture backend.
bool StartVideo(Capture *_cap, const char *_wintitle=0)
Start video input from given capture device.
Definition: CvTestbed.cpp:101
bool SetCalib(const char *calibfile, int _x_res, int _y_res, FILE_FORMAT format=FILE_FORMAT_DEFAULT)
Set the calibration file and the current resolution for which the calibration is adjusted to...
Definition: Camera.cpp:270
std::vector< std::string > CapturePluginVector
Vector of strings.
void SetColor(double _r=1, double _g=1, double _b=1)
Definition: GlutViewer.cpp:19
void outputEnumeratedPlugins(CaptureFactory::CapturePluginVector &plugins)
Definition: Shared.h:8
void SetGlProjectionMatrix(double p[16])
Definition: GlutViewer.cpp:388
void SetKeyCallback(int(*_keycallback)(int key))
Sets the keyboard callback function that will be called when keyboard is pressed. ...
Definition: CvTestbed.cpp:97
OwnDrawable d_points[1000]
void SetGlModelviewMatrix(double p[16])
Definition: GlutViewer.cpp:392
virtual void Draw()
void Reset()
Definition: Pose.cpp:69
CaptureDeviceVector enumerateDevices(const std::string &captureType="")
Enumerate capture devices currently available.
void Start(int argc, char **argv, int w, int h, float r=300.0)
Definition: GlutViewer.cpp:227
int main(int argc, char *argv[])
void DrawableClear()
Definition: GlutViewer.cpp:416
bool Update(IplImage *image, bool assume_plane=true, bool triangulate=true, float reproj_err_limit=5.f, float triangulate_angle=15.f)
Update position assuming that camera is moving with 6-DOF.
Definition: SfM.cpp:154
Pose representation derived from the Rotation class
Definition: Pose.h:50
void DrawableAdd(Drawable *item)
Definition: GlutViewer.cpp:421
CapturePluginVector enumeratePlugins()
Enumerate capture plugins currently available.
void Draw(IplImage *rgba)
Draw debug information about the tracked features and detected markers.
Definition: SfM.cpp:456
XML file format.
Definition: FileFormat.h:66
Pose * GetPose()
Get the estimated pose.
Definition: SfM.cpp:87
std::stringstream calibrationFilename
void outputEnumeratedDevices(CaptureFactory::CaptureDeviceVector &devices, int selectedDevice)
Definition: Shared.h:20
void Invert()
Definition: Pose.cpp:132
void SetVideo(const IplImage *_image)
Definition: GlutViewer.cpp:428
void SetScale(double _scale)
Definition: GlutViewer.cpp:15
SimpleSfM * sfm
bool ToggleImageVisible(size_t index, int flags=1)
Toggle the visibility of the stored image.
Definition: CvTestbed.cpp:180
Capture interface that plugins must implement.
Definition: Capture.h:46
virtual bool loadSettings(std::string filename)
Load camera settings from a file.
Definition: Capture.h:145
int defaultDevice(CaptureFactory::CaptureDeviceVector &devices)
Definition: Shared.h:40
int keycallback(int key)
virtual void setResolution(const unsigned long xResolution, const unsigned long yResolution)
Set the resolution.
Definition: Capture.h:93
void SetTranslation(const CvMat *tra)
Definition: Pose.cpp:150
std::map< int, Feature > container
The map of all tracked features.
Definition: SfM.h:71
void Reset(bool reset_also_triangulated=true)
Reset the situation back to the point it was when SetResetPoint was called.
Definition: SfM.cpp:66
void videocallback(IplImage *image)
virtual bool saveSettings(std::string filename)
Save camera settings to a file.
Definition: Capture.h:124
bool reset


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Jun 6 2019 19:27:24