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  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  if(_ui->groupBox_export_points->isEnabled() && _ui->groupBox_export_points->isChecked())
189  {
190  std::map<int, Transform> posesOut;
191  std::multimap<int, Link> linksOut;
192  Optimizer::Type sbaType = _ui->comboBox_sbaType->currentIndex()==0?Optimizer::kTypeG2O:Optimizer::kTypeCVSBA;
193  UASSERT(Optimizer::isAvailable(sbaType));
194  ParametersMap parametersSBA = parameters;
195  uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(_ui->sba_iterations->value())));
196  uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(_ui->sba_variance->value())));
197  Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
198  sba->getConnectedGraph(poses.begin()->first, poses, links, posesOut, linksOut);
199  poses = sba->optimizeBA(
200  posesOut.begin()->first,
201  posesOut,
202  linksOut,
203  signatures.toStdMap(),
204  points3DMap,
205  wordReferences,
206  _ui->sba_rematchFeatures->isChecked());
207  delete sba;
208 
209  if(poses.empty())
210  {
211  QMessageBox::warning(this, tr("Exporting cameras..."), tr("SBA optimization failed! Cannot export with 3D points.").arg(path));
212  return;
213  }
214  }
215 
216  // export cameras and images
217  QFile fileOut(path+QDir::separator()+"cameras.out");
218  QFile fileList(path+QDir::separator()+"list.txt");
219  QFile fileListKeys(path+QDir::separator()+"list_keys.txt");
220  QDir(path).mkdir("images");
221  if(wordReferences.size())
222  {
223  QDir(path).mkdir("keys");
224  }
225  if(fileOut.open(QIODevice::WriteOnly | QIODevice::Text))
226  {
227  if(fileList.open(QIODevice::WriteOnly | QIODevice::Text))
228  {
229  std::map<int, Transform> cameras;
230  std::map<int, int> cameraIndexes;
231  int camIndex = 0;
232  std::map<int, QColor> colors;
233  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
234  {
235  QMap<int, Signature>::const_iterator ster = signatures.find(iter->first);
236  if(ster!= signatures.end())
237  {
238  cv::Mat image = ster.value().sensorData().imageRaw();
239  if(image.empty())
240  {
241  ster.value().sensorData().uncompressDataConst(&image, 0, 0, 0);
242  }
243 
244  double maxLinearVel = _ui->doubleSpinBox_linearSpeed->value();
245  double maxAngularVel = _ui->doubleSpinBox_angularSpeed->value();
246  double laplacianThr = _ui->doubleSpinBox_laplacianVariance->value();
247  bool blurryImage = false;
248  const std::vector<float> & velocity = ster.value().getVelocity();
249  if(maxLinearVel>0.0 || maxAngularVel>0.0)
250  {
251  if(velocity.size() == 6)
252  {
253  float transVel = uMax3(fabs(velocity[0]), fabs(velocity[1]), fabs(velocity[2]));
254  float rotVel = uMax3(fabs(velocity[3]), fabs(velocity[4]), fabs(velocity[5]));
255  if(maxLinearVel>0.0 && transVel > maxLinearVel)
256  {
257  UWARN("Fast motion detected for camera %d (speed=%f m/s > thr=%f m/s), camera is ignored for texturing.", iter->first, transVel, maxLinearVel);
258  blurryImage = true;
259  }
260  else if(maxAngularVel>0.0 && rotVel > maxAngularVel)
261  {
262  UWARN("Fast motion detected for camera %d (speed=%f rad/s > thr=%f rad/s), camera is ignored for texturing.", iter->first, rotVel, maxAngularVel);
263  blurryImage = true;
264  }
265  }
266  else
267  {
268  UWARN("Camera motion filtering is set, but velocity of camera %d is not available.", iter->first);
269  }
270  }
271 
272  if(!blurryImage && !image.empty() && laplacianThr>0.0)
273  {
274  cv::Mat imgLaplacian;
275  cv::Laplacian(image, imgLaplacian, CV_16S);
276  cv::Mat m, s;
277  cv::meanStdDev(imgLaplacian, m, s);
278  double stddev_pxl = s.at<double>(0);
279  double var = stddev_pxl*stddev_pxl;
280  if(var < laplacianThr)
281  {
282  blurryImage = true;
283  UWARN("Camera's image %d is detected as blurry (var=%f < thr=%f), camera is ignored for texturing.", iter->first, var, laplacianThr);
284  }
285  }
286  if(!blurryImage)
287  {
288  cameras.insert(*iter);
289  cameraIndexes.insert(std::make_pair(iter->first, camIndex++));
290  QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
291  p = path+QDir::separator()+p;
292  if(cv::imwrite(p.toStdString(), image))
293  {
294  UINFO("saved image %s", p.toStdString().c_str());
295  }
296  else
297  {
298  UERROR("Failed to save image %s", p.toStdString().c_str());
299  }
300 
301  //
302  // Descriptors
303  //
304  // The file format starts with 2 integers giving the total number of
305  // keypoints and the length of the descriptor vector for each keypoint
306  // (128). Then the location of each keypoint in the image is specified by
307  // 4 floating point numbers giving subpixel row and column location,
308  // scale, and orientation (in radians from -PI to PI). Obviously, these
309  // numbers are not invariant to viewpoint, but can be used in later
310  // stages of processing to check for geometric consistency among matches.
311  // Finally, the invariant descriptor vector for the keypoint is given as
312  // a list of 128 integers in range [0,255]. Keypoints from a new image
313  // can be matched to those from previous images by simply looking for the
314  // descriptor vector with closest Euclidean distance among all vectors
315  // from previous images.
316  //
317  if(wordReferences.size())
318  {
319  std::list<FeatureBA> descriptors;
320  for(std::map<int, std::map<int, FeatureBA> >::iterator jter=wordReferences.begin(); jter!=wordReferences.end(); ++jter)
321  {
322  for(std::map<int, FeatureBA>::iterator kter=jter->second.begin(); kter!=jter->second.end(); ++kter)
323  {
324  if(kter->first == iter->first)
325  {
326  if(!kter->second.descriptor.empty())
327  {
328  descriptors.push_back(kter->second);
329  }
330 
331  if(colors.find(jter->first) == colors.end())
332  {
333  if(!image.empty() &&
334  kter->second.kpt.pt.x >= 0.0f && (int)kter->second.kpt.pt.x < image.cols &&
335  kter->second.kpt.pt.y >= 0.0f && (int)kter->second.kpt.pt.y < image.rows)
336  {
337  UASSERT(image.type() == CV_8UC3 || image.type() == CV_8UC1);
338  QColor c;
339  if(image.channels() == 3)
340  {
341  cv::Vec3b & pixel = image.at<cv::Vec3b>((int)kter->second.kpt.pt.y, (int)kter->second.kpt.pt.x);
342  c.setRgb(pixel[2], pixel[1], pixel[0]);
343  }
344  else // grayscale
345  {
346  unsigned char & pixel = image.at<unsigned char>((int)kter->second.kpt.pt.y, (int)kter->second.kpt.pt.x);
347  c.setRgb(pixel, pixel, pixel);
348  }
349  colors.insert(std::make_pair(jter->first, c));
350  }
351  }
352  }
353  }
354  }
355 
356  QString p = QString("keys")+QDir::separator()+tr("%1.key").arg(iter->first);
357  p = path+QDir::separator()+p;
358  QFile fileKey(p);
359  if(fileKey.open(QIODevice::WriteOnly | QIODevice::Text))
360  {
361  if(descriptors.size())
362  {
363  QTextStream key(&fileKey);
364  key << descriptors.size() << " " << descriptors.front().descriptor.cols << "\n";
365  for(std::list<FeatureBA>::iterator dter=descriptors.begin(); dter!=descriptors.end(); ++dter)
366  {
367  // unpack octave value to get the scale set by SIFT (https://github.com/opencv/opencv/issues/4554)
368  int octave = dter->kpt.octave & 255;
369  octave = octave < 128 ? octave : (-128 | octave);
370  float scale = octave >= 0 ? 1.f/(1 << octave) : (float)(1 << -octave);
371 
372  key << dter->kpt.pt.x << " " << dter->kpt.pt.y << " " << scale << " " << dter->kpt.angle << "\n";
373  for(int i=0; i<dter->descriptor.cols; ++i)
374  {
375  if(dter->descriptor.type() == CV_8U)
376  {
377  key << " " << (int)dter->descriptor.at<unsigned char>(i);
378  }
379  else // assume CV_32F
380  {
381  key << " " << (int)dter->descriptor.at<float>(i);
382  }
383  if((i+1)%20 == 0 && i+1 < dter->descriptor.cols)
384  {
385  key << "\n";
386  }
387  }
388  key << "\n";
389  }
390  }
391  else
392  {
393  UWARN("No descriptors saved for frame %d in file %s. "
394  "Descriptors may not have been saved in the nodes. "
395  "Verify that parameter %s was true during mapping.",
396  iter->first, p.toStdString().c_str(),
397  Parameters::kMemRawDescriptorsKept().c_str());
398  }
399  fileKey.close();
400  }
401  }
402  }
403  }
404  else
405  {
406  UWARN("Could not find node data for pose %d", iter->first);
407  }
408  }
409 
410  static const Transform opengl_world_T_rtabmap_world(
411  0.0f, -1.0f, 0.0f, 0.0f,
412  0.0f, 0.0f, 1.0f, 0.0f,
413  -1.0f, 0.0f, 0.0f, 0.0f);
414 
415  static const Transform optical_rotation_inv(
416  0.0f, -1.0f, 0.0f, 0.0f,
417  0.0f, 0.0f, -1.0f, 0.0f,
418  1.0f, 0.0f, 0.0f, 0.0f);
419 
420  QTextStream out(&fileOut);
421  QTextStream list(&fileList);
422  out << "# Bundle file v0.3\n";
423  out << cameras.size() << " " << points3DMap.size() << "\n";
424 
425  //
426  // Each camera entry <cameraI> contains the estimated camera intrinsics and extrinsics, and has the form:
427  //
428  // <f> <k1> <k2> [the focal length, followed by two radial distortion coeffs]
429  // <R> [a 3x3 matrix representing the camera rotation]
430  // <t> [a 3-vector describing the camera translation]
431  //
432  // The cameras are specified in the order they appear in the list of images.
433  //
434  for(std::map<int, Transform>::iterator iter=cameras.begin(); iter!=cameras.end(); ++iter)
435  {
436  QString p = QString("images")+QDir::separator()+tr("%1.jpg").arg(iter->first);
437  list << p << "\n";
438 
439  Transform localTransform;
440  QMap<int, Signature>::const_iterator ster = signatures.find(iter->first);
441  UASSERT(ster!=signatures.end());
442  if(ster.value().sensorData().cameraModels().size())
443  {
444  out << ster.value().sensorData().cameraModels().at(0).fx() << " 0 0\n";
445  localTransform = ster.value().sensorData().cameraModels().at(0).localTransform();
446  }
447  else if(ster.value().sensorData().stereoCameraModels().size())
448  {
449  out << ster.value().sensorData().stereoCameraModels()[0].left().fx() << " 0 0\n";
450  localTransform = ster.value().sensorData().stereoCameraModels()[0].left().localTransform();
451  }
452 
453  Transform pose = iter->second;
454  if(!localTransform.isNull())
455  {
456  pose*=localTransform*optical_rotation_inv;
457  }
458  Transform poseGL = opengl_world_T_rtabmap_world*pose.inverse();
459 
460  out << poseGL.r11() << " " << poseGL.r12() << " " << poseGL.r13() << "\n";
461  out << poseGL.r21() << " " << poseGL.r22() << " " << poseGL.r23() << "\n";
462  out << poseGL.r31() << " " << poseGL.r32() << " " << poseGL.r33() << "\n";
463  out << poseGL.x() << " " << poseGL.y() << " " << poseGL.z() << "\n";
464  }
465  if(wordReferences.size())
466  {
467  if(fileListKeys.open(QIODevice::WriteOnly | QIODevice::Text))
468  {
469  QTextStream listKeys(&fileListKeys);
470  for(std::map<int, Transform>::iterator iter=cameras.begin(); iter!=cameras.end(); ++iter)
471  {
472  QString p = QString("keys")+QDir::separator()+tr("%1.key").arg(iter->first);
473  listKeys << p << "\n";
474  }
475  fileListKeys.close();
476  }
477  }
478 
479  //
480  // Each point entry has the form:
481  //
482  // <position> [a 3-vector describing the 3D position of the point]
483  // <color> [a 3-vector describing the RGB color of the point]
484  // <view list> [a list of views the point is visible in]
485  //
486  // The view list begins with the length of the list (i.e., the number of cameras
487  // the point is visible in). The list is then given as a list of quadruplets
488  // <camera> <key> <x> <y>, where <camera> is a camera index, <key> the index
489  // of the SIFT keypoint where the point was detected in that camera, and <x>
490  // and <y> are the detected positions of that keypoint. Both indices are
491  // 0-based (e.g., if camera 0 appears in the list, this corresponds to the
492  // first camera in the scene file and the first image in "list.txt"). The
493  // pixel positions are floating point numbers in a coordinate system where
494  // the origin is the center of the image, the x-axis increases to the right,
495  // and the y-axis increases towards the top of the image. Thus, (-w/2, -h/2)
496  // is the lower-left corner of the image, and (w/2, h/2) is the top-right
497  // corner (where w and h are the width and height of the image).
498  //
499  std::map<int, int> descriptorIndexes;
500  for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
501  {
502  std::map<int, std::map<int, FeatureBA> >::iterator jter = wordReferences.find(iter->first);
503  out << iter->second.x << " " << iter->second.y << " " << iter->second.z << "\n";
504  UASSERT(colors.find(iter->first) != colors.end());
505  QColor & c = colors.at(iter->first);
506  out << c.red() << " " << c.green() << " " << c.blue() << "\n";
507  out << jter->second.size();
508  for(std::map<int, FeatureBA>::iterator kter = jter->second.begin(); kter!=jter->second.end(); ++kter)
509  {
510  // <camera> <key> <x> <y>
511  int camId = kter->first;
512  UASSERT(signatures.contains(camId));
513  UASSERT(cameraIndexes.find(camId) != cameraIndexes.end());
514  const Signature & s = signatures[camId];
515  cv::Point2f pt;
516  if(signatures[camId].sensorData().cameraModels().size())
517  {
518  pt.x = kter->second.kpt.pt.x - s.sensorData().cameraModels().at(0).cx();
519  pt.y = kter->second.kpt.pt.y - s.sensorData().cameraModels().at(0).cy();
520  }
521  else if(signatures[camId].sensorData().stereoCameraModels().size())
522  {
523  pt.x = kter->second.kpt.pt.x - s.sensorData().stereoCameraModels()[0].left().cx();
524  pt.y = kter->second.kpt.pt.y - s.sensorData().stereoCameraModels()[0].left().cy();
525  }
526  descriptorIndexes.insert(std::make_pair(camId, 0));
527  out << " " << cameraIndexes.at(camId) << " " << descriptorIndexes.at(camId)++ << " " << pt.x << " " << -pt.y;
528  }
529  out << "\n";
530  }
531 
532  fileList.close();
533  fileOut.close();
534 
535  QMessageBox::information(this,
536  tr("Exporting cameras in Bundler format..."),
537  tr("%1 cameras/images and %2 points exported to directory \"%3\".%4")
538  .arg(poses.size())
539  .arg(points3DMap.size())
540  .arg(path)
541  .arg(poses.size()>cameras.size()?tr(" %1/%2 cameras ignored for too fast motion and/or blur level.").arg(poses.size()-cameras.size()).arg(poses.size()):""));
542  }
543  else
544  {
545  fileOut.close();
546  QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"list.txt"));
547  }
548  }
549  else
550  {
551  QMessageBox::warning(this, tr("Exporting cameras..."), tr("Failed opening file %1 for writing.").arg(path+QDir::separator()+"cameras.out"));
552  }
553  }
554 }
555 
556 }
rtabmap::Optimizer::isAvailable
static bool isAvailable(Optimizer::Type type)
Definition: Optimizer.cpp:47
int
int
rtabmap::ExportBundlerDialog::exportBundler
void exportBundler(std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const QMap< int, Signature > &signatures, const ParametersMap &parameters)
Definition: ExportBundlerDialog.cpp:167
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)
cameras
std::array< PinholeCamera< Cal3_S2 >, 3 > cameras
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
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 Mon Apr 28 2025 02:45:53