OptimizerCVSBA.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 #include "rtabmap/core/Graph.h"
28 
30 #include <rtabmap/utilite/UStl.h>
31 #include <rtabmap/utilite/UMath.h>
33 #include <rtabmap/utilite/UTimer.h>
34 #include <set>
35 
37 
38 #ifdef RTABMAP_CVSBA
39 #include <cvsba/cvsba.h>
43 #endif
44 
45 namespace rtabmap {
46 
48 {
49 #ifdef RTABMAP_CVSBA
50  return true;
51 #else
52  return false;
53 #endif
54 }
55 
56 std::map<int, Transform> OptimizerCVSBA::optimizeBA(
57  int rootId,
58  const std::map<int, Transform> & posesIn,
59  const std::multimap<int, Link> & links,
60  const std::map<int, std::vector<CameraModel> > & models,
61  std::map<int, cv::Point3f> & points3DMap,
62  const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
63  std::set<int> * outliers)
64 {
65 #ifdef RTABMAP_CVSBA
66  // run sba optimization
67  cvsba::Sba sba;
68 
69  std::map<int, Transform> poses(posesIn.lower_bound(1), posesIn.end());
70 
71  // change params if desired
72  cvsba::Sba::Params params ;
73  params.type = cvsba::Sba::MOTIONSTRUCTURE;
74  params.iterations = this->iterations();
75  params.minError = this->epsilon();
76  params.fixedIntrinsics = 5;
77  params.fixedDistortion = 5; // updated below
78  params.verbose=ULogger::level() <= ULogger::kInfo;
79  sba.setParams(params);
80 
81  std::vector<cv::Mat> cameraMatrix(poses.size()); //nframes
82  std::vector<cv::Mat> R(poses.size()); //nframes
83  std::vector<cv::Mat> T(poses.size()); //nframes
84  std::vector<cv::Mat> distCoeffs(poses.size()); //nframes
85  std::map<int, int> frameIdToIndex;
86  int oi=0;
87  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
88  {
89  // Get camera model
90  std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
91  UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
92 
93  frameIdToIndex.insert(std::make_pair(iter->first, oi));
94 
95  cameraMatrix[oi] = iterModel->second.K();
96  if(iterModel->second.D().cols != 5)
97  {
98  distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
99  UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
100  }
101  else
102  {
103  distCoeffs[oi] = iterModel->second.D();
104  }
105 
106  Transform t = (iter->second * iterModel->second.localTransform()).inverse();
107 
108  R[oi] = (cv::Mat_<double>(3,3) <<
109  (double)t.r11(), (double)t.r12(), (double)t.r13(),
110  (double)t.r21(), (double)t.r22(), (double)t.r23(),
111  (double)t.r31(), (double)t.r32(), (double)t.r33());
112  T[oi] = (cv::Mat_<double>(1,3) << (double)t.x(), (double)t.y(), (double)t.z());
113  ++oi;
114 
115  UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str());
116  }
117  cameraMatrix.resize(oi);
118  R.resize(oi);
119  T.resize(oi);
120  distCoeffs.resize(oi);
121 
122  UDEBUG("points=%d frames=%d", (int)points3DMap.size(), (int)poses.size());
123  std::vector<cv::Point3f> points(points3DMap.size()); //npoints
124  std::vector<std::vector<cv::Point2f> > imagePoints(poses.size()); //nframes -> npoints
125  std::vector<std::vector<int> > visibility(poses.size()); //nframes -> npoints
126  for(unsigned int i=0; i<poses.size(); ++i)
127  {
128  imagePoints[i].resize(wordReferences.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
129  visibility[i].resize(wordReferences.size(), 0);
130  }
131  int i=0;
132  for(std::map<int, cv::Point3f>::const_iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
133  {
134  points[i] = kter->second;
135 
136  std::map<int, std::map<int, FeatureBA> >::const_iterator iter = wordReferences.find(kter->first);
137  if(iter != wordReferences.end())
138  {
139  for(std::map<int, FeatureBA>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
140  {
141  if(frameIdToIndex.find(jter->first) != frameIdToIndex.end())
142  {
143  imagePoints[frameIdToIndex.at(jter->first)][i] = cv::Point2f(jter->second.kpt.pt.x, jter->second.kpt.pt.y);
144  visibility[frameIdToIndex.at(jter->first)][i] = 1;
145  }
146  }
147  }
148  ++i;
149  }
150 
151  // SBA
152  try
153  {
154  sba.run( points, imagePoints, visibility, cameraMatrix, R, T, distCoeffs);
155  }
156  catch(cv::Exception & e)
157  {
158  UERROR("Running SBA... error! %s", e.what());
159  return std::map<int, Transform>();
160  }
161 
162  //update poses
163  i=0;
164  std::map<int, Transform> newPoses = poses;
165  for(std::map<int, Transform>::iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
166  {
167  Transform t(R[i].at<double>(0,0), R[i].at<double>(0,1), R[i].at<double>(0,2), T[i].at<double>(0),
168  R[i].at<double>(1,0), R[i].at<double>(1,1), R[i].at<double>(1,2), T[i].at<double>(1),
169  R[i].at<double>(2,0), R[i].at<double>(2,1), R[i].at<double>(2,2), T[i].at<double>(2));
170 
171  UDEBUG("New pose %d = %s", iter->first, t.prettyPrint().c_str());
172 
173  if(this->isSlam2d())
174  {
175  t = (models.at(iter->first).localTransform() * t).inverse();
176  t = iter->second.inverse() * t;
177  iter->second *= t.to3DoF();
178  }
179  else
180  {
181  iter->second = (models.at(iter->first).localTransform() * t).inverse();
182  }
183 
184  ++i;
185  }
186 
187  //update 3D points
188  i=0;
189  for(std::map<int, cv::Point3f>::iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
190  {
191  kter->second = points[i++];
192  }
193 
194  return newPoses;
195 
196 #else
197  UERROR("RTAB-Map is not built with cvsba!");
198  return std::map<int, Transform>();
199 #endif
200 }
201 
202 } /* namespace rtabmap */
float r11() const
Definition: Transform.h:62
float r33() const
Definition: Transform.h:70
float r23() const
Definition: Transform.h:67
T
float r13() const
Definition: Transform.h:64
bool isSlam2d() const
Definition: Optimizer.h:92
GLM_FUNC_DECL genType e()
double epsilon() const
Definition: Optimizer.h:94
float r32() const
Definition: Transform.h:69
Basic mathematics functions.
Some conversion functions.
float r22() const
Definition: Transform.h:66
#define UASSERT(condition)
Wrappers of STL for convenient functions.
std::string prettyPrint() const
Definition: Transform.cpp:316
int iterations() const
Definition: Optimizer.h:91
static ULogger::Level level()
Definition: ULogger.h:340
params
float r21() const
Definition: Transform.h:65
float r12() const
Definition: Transform.h:63
#define UDEBUG(...)
#define UERROR(...)
ULogger class and convenient macros.
#define UWARN(...)
float r31() const
Definition: Transform.h:68
Transform to3DoF() const
Definition: Transform.cpp:210
Transform inverse() const
Definition: Transform.cpp:178
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)
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29