ExportBundlerDialog.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/gui/ExportBundlerDialog.h"
29 #include "ui_exportBundlerDialog.h"
30 #include <rtabmap/utilite/UMath.h>
31 #include <rtabmap/utilite/UStl.h>
32 #include <rtabmap/core/Optimizer.h>
33 #include <rtabmap/core/util3d_transforms.h>
34 #include <QFileDialog>
35 #include <QPushButton>
36 #include <QMessageBox>
37 #include <QTextStream>
38 
39 namespace rtabmap {
40 
41 ExportBundlerDialog::ExportBundlerDialog(QWidget * parent) :
42  QDialog(parent)
43 {
44  _ui = new Ui_ExportBundlerDialog();
45  _ui->setupUi(this);
46 
47  connect(_ui->toolButton_path, SIGNAL(clicked()), this, SLOT(getPath()));
48 
49  restoreDefaults();
50  connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
51 
52  connect(_ui->doubleSpinBox_laplacianVariance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
53  connect(_ui->doubleSpinBox_linearSpeed, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
54  connect(_ui->doubleSpinBox_angularSpeed, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
55  connect(_ui->groupBox_export_points, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
56  connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
57  connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
58  connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateVisibility()));
59  connect(_ui->sba_rematchFeatures, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
60 
61  if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA) && !Optimizer::isAvailable(Optimizer::kTypeG2O))
62  {
63  _ui->groupBox_export_points->setEnabled(false);
64  _ui->groupBox_export_points->setChecked(false);
65  }
66  else if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
67  {
68  _ui->comboBox_sbaType->setItemData(1, 0, Qt::UserRole - 1);
69  _ui->comboBox_sbaType->setCurrentIndex(0);
70  }
71  else if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
72  {
73  _ui->comboBox_sbaType->setItemData(0, 0, Qt::UserRole - 1);
74  _ui->comboBox_sbaType->setCurrentIndex(1);
75  }
76 
77  _ui->lineEdit_path->setText(QDir::currentPath());
78 
79  updateVisibility();
80 }
81 
82 ExportBundlerDialog::~ExportBundlerDialog()
83 {
84  delete _ui;
85 }
86 
87 void ExportBundlerDialog::saveSettings(QSettings & settings, const QString & group) const
88 {
89  if(!group.isEmpty())
90  {
91  settings.beginGroup(group);
92  }
93  settings.setValue("maxLinearSpeed", _ui->doubleSpinBox_linearSpeed->value());
94  settings.setValue("maxAngularSpeed", _ui->doubleSpinBox_angularSpeed->value());
95  settings.setValue("laplacianThr", _ui->doubleSpinBox_laplacianVariance->value());
96  settings.setValue("exportPoints", _ui->groupBox_export_points->isChecked());
97  settings.setValue("sba_iterations", _ui->sba_iterations->value());
98  settings.setValue("sba_type", _ui->comboBox_sbaType->currentIndex());
99  settings.setValue("sba_variance", _ui->sba_variance->value());
100  settings.setValue("sba_rematch_features", _ui->sba_rematchFeatures->isChecked());
101  if(!group.isEmpty())
102  {
103  settings.endGroup();
104  }
105 }
106 
107 void ExportBundlerDialog::loadSettings(QSettings & settings, const QString & group)
108 {
109  if(!group.isEmpty())
110  {
111  settings.beginGroup(group);
112  }
113  _ui->doubleSpinBox_linearSpeed->setValue(settings.value("maxLinearSpeed", _ui->doubleSpinBox_linearSpeed->value()).toDouble());
114  _ui->doubleSpinBox_angularSpeed->setValue(settings.value("maxAngularSpeed", _ui->doubleSpinBox_angularSpeed->value()).toDouble());
115  _ui->doubleSpinBox_laplacianVariance->setValue(settings.value("laplacianThr", _ui->doubleSpinBox_laplacianVariance->value()).toDouble());
116  _ui->groupBox_export_points->setChecked(settings.value("exportPoints", _ui->groupBox_export_points->isChecked()).toBool());
117  _ui->sba_iterations->setValue(settings.value("sba_iterations", _ui->sba_iterations->value()).toInt());
118  _ui->comboBox_sbaType->setCurrentIndex((Optimizer::Type)settings.value("sba_type", _ui->comboBox_sbaType->currentIndex()).toInt());
119  _ui->sba_variance->setValue(settings.value("sba_variance", _ui->sba_variance->value()).toDouble());
120  _ui->sba_rematchFeatures->setChecked(settings.value("sba_rematch_features", _ui->sba_rematchFeatures->isChecked()).toBool());
121  if(!group.isEmpty())
122  {
123  settings.endGroup();
124  }
125 }
126 
127 void ExportBundlerDialog::setWorkingDirectory(const QString & path)
128 {
129  _ui->lineEdit_path->setText((path.isEmpty()?QDir::currentPath():path) + "/bundler");
130 }
131 
132 void ExportBundlerDialog::restoreDefaults()
133 {
134  _ui->doubleSpinBox_linearSpeed->setValue(0);
135  _ui->doubleSpinBox_angularSpeed->setValue(0);
136  _ui->doubleSpinBox_laplacianVariance->setValue(0);
137  _ui->groupBox_export_points->setChecked(false);
138  _ui->sba_iterations->setValue(20);
139  if(Optimizer::isAvailable(Optimizer::kTypeG2O) || !Optimizer::isAvailable(Optimizer::kTypeCVSBA))
140  {
141  _ui->comboBox_sbaType->setCurrentIndex(0);
142  }
143  else
144  {
145  _ui->comboBox_sbaType->setCurrentIndex(1);
146  }
147 
148  _ui->sba_variance->setValue(1.0);
149  _ui->sba_rematchFeatures->setChecked(true);
150 }
151 
152 void ExportBundlerDialog::updateVisibility()
153 {
154  _ui->sba_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
155  _ui->label_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
156 }
157 
158 void ExportBundlerDialog::getPath()
159 {
160  QString path = QFileDialog::getExistingDirectory(this, tr("Exporting cameras in Bundler format..."), _ui->lineEdit_path->text());
161  if(!path.isEmpty())
162  {
163  _ui->lineEdit_path->setText(path);
164  }
165 }
166 
167 void ExportBundlerDialog::exportBundler(
168  const std::map<int, Transform> & poses,
169  const std::multimap<int, Link> & links,
170  const QMap<int, Signature> & signatures,
171  const ParametersMap & parameters)
172 {
173  if(this->exec() != QDialog::Accepted)
174  {
175  return;
176  }
177  QString path = _ui->lineEdit_path->text();
178  if(!path.isEmpty())
179  {
180  if(!QDir(path).mkpath("."))
181  {
182  QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed creating directory %1.").arg(path));
183  return;
184  }
185 
186  std::map<int, cv::Point3f> points3DMap;
187  std::map<int, std::map<int, FeatureBA> > wordReferences;
188  std::map<int, Transform> newPoses = poses;
189  if(_ui->groupBox_export_points->isEnabled() && _ui->groupBox_export_points->isChecked())
190  {
191  std::map<int, Transform> posesOut;
192  std::multimap<int, Link> linksOut;
193  Optimizer::Type sbaType = _ui->comboBox_sbaType->currentIndex()==0?Optimizer::kTypeG2O:Optimizer::kTypeCVSBA;
194  UASSERT(Optimizer::isAvailable(sbaType));
195  ParametersMap parametersSBA = parameters;
196  uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(_ui->sba_iterations->value())));
197  uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(_ui->sba_variance->value())));
198  Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
199  sba->getConnectedGraph(poses.begin()->first, poses, links, posesOut, linksOut);
200  newPoses = sba->optimizeBA(
201  posesOut.begin()->first,
202  posesOut,
203  linksOut,
204  signatures.toStdMap(),
205  points3DMap,
206  wordReferences,
207  _ui->sba_rematchFeatures->isChecked());
208  delete sba;
209 
210  if(newPoses.empty())
211  {
212  QMessageBox::warning(this, tr("Exporting cameras..."), tr("SBA optimization failed! Cannot export with 3D points.").arg(path));
213  return;
214  }
215  }
216 
217  // export cameras and images
218  QFile fileOut(path+QDir::separator()+"cameras.out");
219  QFile fileList(path+QDir::separator()+"list.txt");
220  QFile fileListKeys(path+QDir::separator()+"list_keys.txt");
221  QDir(path).mkdir("images");
222  if(wordReferences.size())
223  {
224  QDir(path).mkdir("keys");
225  }
226  if(fileOut.open(QIODevice::WriteOnly | QIODevice::Text))
227  {
228  if(fileList.open(QIODevice::WriteOnly | QIODevice::Text))
229  {
230  std::map<int, Transform> cameras;
231  std::map<int, int> cameraIndexes;
232  int camIndex = 0;
233  std::map<int, QColor> colors;
234  for(std::map<int, Transform>::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
235  {
236  QMap<int, Signature>::const_iterator ster = signatures.find(iter->first);
237  if(ster!= signatures.end())
238  {
239  cv::Mat image = ster.value().sensorData().imageRaw();
240  if(image.empty())
241  {
242  ster.value().sensorData().uncompressDataConst(&image, 0, 0, 0);
243  }
244 
245  double maxLinearVel = _ui->doubleSpinBox_linearSpeed->value();
246  double maxAngularVel = _ui->doubleSpinBox_angularSpeed->value();
247  double laplacianThr = _ui->doubleSpinBox_laplacianVariance->value();
248  bool blurryImage = false;
249  const std::vector<float> & velocity = ster.value().getVelocity();
250  if(maxLinearVel>0.0 || maxAngularVel>0.0)
251  {
252  if(velocity.size() == 6)
253  {
254  float transVel = uMax3(fabs(velocity[0]), fabs(velocity[1]), fabs(velocity[2]));
255  float rotVel = uMax3(fabs(velocity[3]), fabs(velocity[4]), fabs(velocity[5]));
256  if(maxLinearVel>0.0 && transVel > maxLinearVel)
257  {
258  UWARN("Fast motion detected for camera %d (speed=%f m/s > thr=%f m/s), camera is ignored for texturing.", iter->first, transVel, maxLinearVel);
259  blurryImage = true;
260  }
261  else if(maxAngularVel>0.0 && rotVel > maxAngularVel)
262  {
263  UWARN("Fast motion detected for camera %d (speed=%f rad/s > thr=%f rad/s), camera is ignored for texturing.", iter->first, rotVel, maxAngularVel);
264  blurryImage = true;
265  }
266  }
267  else
268  {
269  UWARN("Camera motion filtering is set, but velocity of camera %d is not available.", iter->first);
270  }
271  }
272 
273  if(!blurryImage && !image.empty() && laplacianThr>0.0)
274  {
275  cv::Mat imgLaplacian;
276  cv::Laplacian(image, imgLaplacian, CV_16S);
277  cv::Mat m, s;
278  cv::meanStdDev(imgLaplacian, m, s);
279  double stddev_pxl = s.at<double>(0);
280  double var = stddev_pxl*stddev_pxl;
281  if(var < laplacianThr)
282  {
283  blurryImage = true;
284  UWARN("Camera's image %d is detected as blurry (var=%f < thr=%f), camera is ignored for texturing.", iter->first, var, laplacianThr);
285  }
286  }
287  if(!blurryImage)
288  {
289  cameras.insert(*iter);
290  cameraIndexes.insert(std::make_pair(iter->first, camIndex++));
291  QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
292  p = path+QDir::separator()+p;
293  if(cv::imwrite(p.toStdString(), image))
294  {
295  UINFO("saved image %s", p.toStdString().c_str());
296  }
297  else
298  {
299  UERROR("Failed to save image %s", p.toStdString().c_str());
300  }
301 
302  //
303  // Descriptors
304  //
305  // The file format starts with 2 integers giving the total number of
306  // keypoints and the length of the descriptor vector for each keypoint
307  // (128). Then the location of each keypoint in the image is specified by
308  // 4 floating point numbers giving subpixel row and column location,
309  // scale, and orientation (in radians from -PI to PI). Obviously, these
310  // numbers are not invariant to viewpoint, but can be used in later
311  // stages of processing to check for geometric consistency among matches.
312  // Finally, the invariant descriptor vector for the keypoint is given as
313  // a list of 128 integers in range [0,255]. Keypoints from a new image
314  // can be matched to those from previous images by simply looking for the
315  // descriptor vector with closest Euclidean distance among all vectors
316  // from previous images.
317  //
318  if(wordReferences.size())
319  {
320  std::list<FeatureBA> descriptors;
321  for(std::map<int, std::map<int, FeatureBA> >::iterator jter=wordReferences.begin(); jter!=wordReferences.end(); ++jter)
322  {
323  for(std::map<int, FeatureBA>::iterator kter=jter->second.begin(); kter!=jter->second.end(); ++kter)
324  {
325  if(kter->first == iter->first)
326  {
327  if(!kter->second.descriptor.empty())
328  {
329  descriptors.push_back(kter->second);
330  }
331 
332  if(colors.find(jter->first) == colors.end())
333  {
334  if(!image.empty() &&
335  kter->second.kpt.pt.x >= 0.0f && (int)kter->second.kpt.pt.x < image.cols &&
336  kter->second.kpt.pt.y >= 0.0f && (int)kter->second.kpt.pt.y < image.rows)
337  {
338  UASSERT(image.type() == CV_8UC3 || image.type() == CV_8UC1);
339  QColor c;
340  if(image.channels() == 3)
341  {
342  cv::Vec3b & pixel = image.at<cv::Vec3b>((int)kter->second.kpt.pt.y, (int)kter->second.kpt.pt.x);
343  c.setRgb(pixel[2], pixel[1], pixel[0]);
344  }
345  else // grayscale
346  {
347  unsigned char & pixel = image.at<unsigned char>((int)kter->second.kpt.pt.y, (int)kter->second.kpt.pt.x);
348  c.setRgb(pixel, pixel, pixel);
349  }
350  colors.insert(std::make_pair(jter->first, c));
351  }
352  }
353  }
354  }
355  }
356 
357  QString p = QString("keys")+QDir::separator()+tr("%1.key").arg(iter->first);
358  p = path+QDir::separator()+p;
359  QFile fileKey(p);
360  if(fileKey.open(QIODevice::WriteOnly | QIODevice::Text))
361  {
362  if(descriptors.size())
363  {
364  QTextStream key(&fileKey);
365  key << descriptors.size() << " " << descriptors.front().descriptor.cols << "\n";
366  for(std::list<FeatureBA>::iterator dter=descriptors.begin(); dter!=descriptors.end(); ++dter)
367  {
368  // unpack octave value to get the scale set by SIFT (https://github.com/opencv/opencv/issues/4554)
369  int octave = dter->kpt.octave & 255;
370  octave = octave < 128 ? octave : (-128 | octave);
371  float scale = octave >= 0 ? 1.f/(1 << octave) : (float)(1 << -octave);
372 
373  key << dter->kpt.pt.x << " " << dter->kpt.pt.y << " " << scale << " " << dter->kpt.angle << "\n";
374  for(int i=0; i<dter->descriptor.cols; ++i)
375  {
376  if(dter->descriptor.type() == CV_8U)
377  {
378  key << " " << (int)dter->descriptor.at<unsigned char>(i);
379  }
380  else // assume CV_32F
381  {
382  key << " " << (int)dter->descriptor.at<float>(i);
383  }
384  if((i+1)%20 == 0 && i+1 < dter->descriptor.cols)
385  {
386  key << "\n";
387  }
388  }
389  key << "\n";
390  }
391  }
392  else
393  {
394  UWARN("No descriptors saved for frame %d in file %s. "
395  "Descriptors may not have been saved in the nodes. "
396  "Verify that parameter %s was true during mapping.",
397  iter->first, p.toStdString().c_str(),
398  Parameters::kMemRawDescriptorsKept().c_str());
399  }
400  fileKey.close();
401  }
402  }
403  }
404  }
405  else
406  {
407  UWARN("Could not find node data for pose %d", iter->first);
408  }
409  }
410 
411  static const Transform opengl_world_T_rtabmap_world(
412  0.0f, -1.0f, 0.0f, 0.0f,
413  0.0f, 0.0f, 1.0f, 0.0f,
414  -1.0f, 0.0f, 0.0f, 0.0f);
415 
416  static const Transform optical_rotation_inv(
417  0.0f, -1.0f, 0.0f, 0.0f,
418  0.0f, 0.0f, -1.0f, 0.0f,
419  1.0f, 0.0f, 0.0f, 0.0f);
420 
421  QTextStream out(&fileOut);
422  QTextStream list(&fileList);
423  out << "# Bundle file v0.3\n";
424  out << cameras.size() << " " << points3DMap.size() << "\n";
425 
426  //
427  // Each camera entry <cameraI> contains the estimated camera intrinsics and extrinsics, and has the form:
428  //
429  // <f> <k1> <k2> [the focal length, followed by two radial distortion coeffs]
430  // <R> [a 3x3 matrix representing the camera rotation]
431  // <t> [a 3-vector describing the camera translation]
432  //
433  // The cameras are specified in the order they appear in the list of images.
434  //
435  for(std::map<int, Transform>::iterator iter=cameras.begin(); iter!=cameras.end(); ++iter)
436  {
437  QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
438  list << p << "\n";
439 
440  Transform localTransform;
441  QMap<int, Signature>::const_iterator ster = signatures.find(iter->first);
442  UASSERT(ster!=signatures.end());
443  if(ster.value().sensorData().cameraModels().size())
444  {
445  out << ster.value().sensorData().cameraModels().at(0).fx() << " 0 0\n";
446  localTransform = ster.value().sensorData().cameraModels().at(0).localTransform();
447  }
448  else if(ster.value().sensorData().stereoCameraModels().size())
449  {
450  out << ster.value().sensorData().stereoCameraModels()[0].left().fx() << " 0 0\n";
451  localTransform = ster.value().sensorData().stereoCameraModels()[0].left().localTransform();
452  }
453 
454  Transform pose = iter->second;
455  if(!localTransform.isNull())
456  {
457  pose*=localTransform*optical_rotation_inv;
458  }
459  Transform poseGL = opengl_world_T_rtabmap_world*pose.inverse();
460 
461  out << poseGL.r11() << " " << poseGL.r12() << " " << poseGL.r13() << "\n";
462  out << poseGL.r21() << " " << poseGL.r22() << " " << poseGL.r23() << "\n";
463  out << poseGL.r31() << " " << poseGL.r32() << " " << poseGL.r33() << "\n";
464  out << poseGL.x() << " " << poseGL.y() << " " << poseGL.z() << "\n";
465  }
466  if(wordReferences.size())
467  {
468  if(fileListKeys.open(QIODevice::WriteOnly | QIODevice::Text))
469  {
470  QTextStream listKeys(&fileListKeys);
471  for(std::map<int, Transform>::iterator iter=cameras.begin(); iter!=cameras.end(); ++iter)
472  {
473  QString p = QString("keys")+QDir::separator()+tr("%1.key").arg(iter->first);
474  listKeys << p << "\n";
475  }
476  fileListKeys.close();
477  }
478  }
479 
480  //
481  // Each point entry has the form:
482  //
483  // <position> [a 3-vector describing the 3D position of the point]
484  // <color> [a 3-vector describing the RGB color of the point]
485  // <view list> [a list of views the point is visible in]
486  //
487  // The view list begins with the length of the list (i.e., the number of cameras
488  // the point is visible in). The list is then given as a list of quadruplets
489  // <camera> <key> <x> <y>, where <camera> is a camera index, <key> the index
490  // of the SIFT keypoint where the point was detected in that camera, and <x>
491  // and <y> are the detected positions of that keypoint. Both indices are
492  // 0-based (e.g., if camera 0 appears in the list, this corresponds to the
493  // first camera in the scene file and the first image in "list.txt"). The
494  // pixel positions are floating point numbers in a coordinate system where
495  // the origin is the center of the image, the x-axis increases to the right,
496  // and the y-axis increases towards the top of the image. Thus, (-w/2, -h/2)
497  // is the lower-left corner of the image, and (w/2, h/2) is the top-right
498  // corner (where w and h are the width and height of the image).
499  //
500  std::map<int, int> descriptorIndexes;
501  for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
502  {
503  std::map<int, std::map<int, FeatureBA> >::iterator jter = wordReferences.find(iter->first);
504  out << iter->second.x << " " << iter->second.y << " " << iter->second.z << "\n";
505  UASSERT(colors.find(iter->first) != colors.end());
506  QColor & c = colors.at(iter->first);
507  out << c.red() << " " << c.green() << " " << c.blue() << "\n";
508  out << jter->second.size();
509  for(std::map<int, FeatureBA>::iterator kter = jter->second.begin(); kter!=jter->second.end(); ++kter)
510  {
511  // <camera> <key> <x> <y>
512  int camId = kter->first;
513  UASSERT(signatures.contains(camId));
514  UASSERT(cameraIndexes.find(camId) != cameraIndexes.end());
515  const Signature & s = signatures[camId];
516  cv::Point2f pt;
517  if(signatures[camId].sensorData().cameraModels().size())
518  {
519  pt.x = kter->second.kpt.pt.x - s.sensorData().cameraModels().at(0).cx();
520  pt.y = kter->second.kpt.pt.y - s.sensorData().cameraModels().at(0).cy();
521  }
522  else if(signatures[camId].sensorData().stereoCameraModels().size())
523  {
524  pt.x = kter->second.kpt.pt.x - s.sensorData().stereoCameraModels()[0].left().cx();
525  pt.y = kter->second.kpt.pt.y - s.sensorData().stereoCameraModels()[0].left().cy();
526  }
527  descriptorIndexes.insert(std::make_pair(camId, 0));
528  out << " " << cameraIndexes.at(camId) << " " << descriptorIndexes.at(camId)++ << " " << pt.x << " " << -pt.y;
529  }
530  out << "\n";
531  }
532 
533  fileList.close();
534  fileOut.close();
535 
536  QMessageBox::information(this,
537  tr("Exporting cameras in Bundler format..."),
538  tr("%1 cameras/images and %2 points exported to directory \"%3\".%4")
539  .arg(newPoses.size())
540  .arg(points3DMap.size())
541  .arg(path)
542  .arg(newPoses.size()>cameras.size()?tr(" %1/%2 cameras ignored for too fast motion and/or blur level.").arg(newPoses.size()-cameras.size()).arg(newPoses.size()):""));
543  }
544  else
545  {
546  fileOut.close();
547  QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"list.txt"));
548  }
549  }
550  else
551  {
552  QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"cameras.out"));
553  }
554  }
555 }
556 
557 }
rtabmap::Optimizer::isAvailable
static bool isAvailable(Optimizer::Type type)
Definition: Optimizer.cpp:47
int
int
UINFO
#define UINFO(...)
rtabmap::Optimizer::optimizeBA
virtual std::map< int, Transform > optimizeBA(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, std::vector< CameraModel > > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, FeatureBA > > &wordReferences, std::set< int > *outliers=0)
Definition: Optimizer.cpp:430
arg::arg
constexpr arg(const char *name=nullptr)
rtabmap::ExportBundlerDialog::~ExportBundlerDialog
virtual ~ExportBundlerDialog()
Definition: ExportBundlerDialog.cpp:82
s
RealScalar s
list
rtabmap::Transform::r33
float r33() const
Definition: Transform.h:70
c
Scalar Scalar * c
size
Index size
rtabmap::Optimizer
Definition: Optimizer.h:61
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
rtabmap::ExportBundlerDialog::_ui
Ui_ExportBundlerDialog * _ui
Definition: ExportBundlerDialog.h:71
iterator
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::Optimizer::kTypeCVSBA
@ kTypeCVSBA
Definition: Optimizer.h:70
rtabmap::ExportBundlerDialog::restoreDefaults
void restoreDefaults()
Definition: ExportBundlerDialog.cpp:132
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UMath.h
Basic mathematics functions.
fabs
Real fabs(const Real &a)
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:441
rtabmap::ExportBundlerDialog::loadSettings
void loadSettings(QSettings &settings, const QString &group="")
Definition: ExportBundlerDialog.cpp:107
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
rtabmap::Transform::z
float & z()
Definition: Transform.h:94
util3d_transforms.h
scale
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
rtabmap::Optimizer::Type
Type
Definition: Optimizer.h:64
rtabmap::Transform::r13
float r13() const
Definition: Transform.h:64
uNumber2Str
std::string UTILITE_EXPORT uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::ExportBundlerDialog::exportBundler
void exportBundler(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const QMap< int, Signature > &signatures, const ParametersMap &parameters)
Definition: ExportBundlerDialog.cpp:167
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:78
rtabmap::ExportBundlerDialog::getPath
void getPath()
Definition: ExportBundlerDialog.cpp:158
rtabmap::opengl_world_T_rtabmap_world
static const rtabmap::Transform opengl_world_T_rtabmap_world(0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f)
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
arg
Optimizer.h
velocity
Velocity3 velocity(const NavState &X, OptionalJacobian< 3, 9 > H)
rtabmap::Transform::r21
float r21() const
Definition: Transform.h:65
UASSERT
#define UASSERT(condition)
m
Matrix3f m
p
Point3_ p(2)
rtabmap::Transform::r31
float r31() const
Definition: Transform.h:68
out
std::ofstream out("Result.txt")
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
path
path
key
const gtsam::Symbol key( 'X', 0)
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::ExportBundlerDialog::ExportBundlerDialog
ExportBundlerDialog(QWidget *parent=0)
Definition: ExportBundlerDialog.cpp:41
rtabmap::Transform::r22
float r22() const
Definition: Transform.h:66
rtabmap::Transform
Definition: Transform.h:41
iter
iterator iter(handle obj)
UStl.h
Wrappers of STL for convenient functions.
ExportBundlerDialog.h
rtabmap::Optimizer::kTypeG2O
@ kTypeG2O
Definition: Optimizer.h:67
rtabmap::Transform::r23
float r23() const
Definition: Transform.h:67
rtabmap::ExportBundlerDialog::configChanged
void configChanged()
rtabmap::Transform::r12
float r12() const
Definition: Transform.h:63
exec
void exec(const char(&s)[N], object global=globals(), object local=object())
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::ExportBundlerDialog::setWorkingDirectory
void setWorkingDirectory(const QString &path)
Definition: ExportBundlerDialog.cpp:127
rtabmap::Transform::r32
float r32() const
Definition: Transform.h:69
UERROR
#define UERROR(...)
rtabmap::ExportBundlerDialog::updateVisibility
void updateVisibility()
Definition: ExportBundlerDialog.cpp:152
i
int i
rtabmap::ExportBundlerDialog::saveSettings
void saveSettings(QSettings &settings, const QString &group="") const
Definition: ExportBundlerDialog.cpp:87
rtabmap::Signature
Definition: Signature.h:48
rtabmap::Optimizer::getConnectedGraph
void getConnectedGraph(int fromId, const std::map< int, Transform > &posesIn, const std::multimap< int, Link > &linksIn, std::map< int, Transform > &posesOut, std::multimap< int, Link > &linksOut) const
Definition: Optimizer.cpp:188
rtabmap::Transform::r11
float r11() const
Definition: Transform.h:62

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:09