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
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, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
91  UASSERT(iterModel != models.end());
92  if(iterModel->second.size() != 1)
93  {
94  UERROR("Multi-camera BA not implemented for cvsba, only single camera.");
95  return std::map<int, Transform>();
96  }
97  UASSERT(iterModel->second[0].isValidForProjection());
98 
99  frameIdToIndex.insert(std::make_pair(iter->first, oi));
100 
101  cameraMatrix[oi] = iterModel->second[0].K();
102  if(iterModel->second[0].D().cols != 5)
103  {
104  distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
105  UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
106  }
107  else
108  {
109  distCoeffs[oi] = iterModel->second[0].D();
110  }
111 
112  Transform t = (iter->second * iterModel->second[0].localTransform()).inverse();
113 
114  R[oi] = (cv::Mat_<double>(3,3) <<
115  (double)t.r11(), (double)t.r12(), (double)t.r13(),
116  (double)t.r21(), (double)t.r22(), (double)t.r23(),
117  (double)t.r31(), (double)t.r32(), (double)t.r33());
118  T[oi] = (cv::Mat_<double>(1,3) << (double)t.x(), (double)t.y(), (double)t.z());
119  ++oi;
120 
121  UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str());
122  }
123  cameraMatrix.resize(oi);
124  R.resize(oi);
125  T.resize(oi);
126  distCoeffs.resize(oi);
127 
128  UDEBUG("points=%d frames=%d", (int)points3DMap.size(), (int)poses.size());
129  std::vector<cv::Point3f> points(points3DMap.size()); //npoints
130  std::vector<std::vector<cv::Point2f> > imagePoints(poses.size()); //nframes -> npoints
131  std::vector<std::vector<int> > visibility(poses.size()); //nframes -> npoints
132  for(unsigned int i=0; i<poses.size(); ++i)
133  {
134  imagePoints[i].resize(wordReferences.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
135  visibility[i].resize(wordReferences.size(), 0);
136  }
137  int i=0;
138  for(std::map<int, cv::Point3f>::const_iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
139  {
140  points[i] = kter->second;
141 
142  std::map<int, std::map<int, FeatureBA> >::const_iterator iter = wordReferences.find(kter->first);
143  if(iter != wordReferences.end())
144  {
145  for(std::map<int, FeatureBA>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
146  {
147  if(frameIdToIndex.find(jter->first) != frameIdToIndex.end())
148  {
149  imagePoints[frameIdToIndex.at(jter->first)][i] = cv::Point2f(jter->second.kpt.pt.x, jter->second.kpt.pt.y);
150  visibility[frameIdToIndex.at(jter->first)][i] = 1;
151  }
152  }
153  }
154  ++i;
155  }
156 
157  // SBA
158  try
159  {
160  sba.run( points, imagePoints, visibility, cameraMatrix, R, T, distCoeffs);
161  }
162  catch(cv::Exception & e)
163  {
164  UERROR("Running SBA... error! %s", e.what());
165  return std::map<int, Transform>();
166  }
167 
168  //update poses
169  i=0;
170  std::map<int, Transform> newPoses = poses;
171  for(std::map<int, Transform>::iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
172  {
173  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),
174  R[i].at<double>(1,0), R[i].at<double>(1,1), R[i].at<double>(1,2), T[i].at<double>(1),
175  R[i].at<double>(2,0), R[i].at<double>(2,1), R[i].at<double>(2,2), T[i].at<double>(2));
176 
177  UDEBUG("New pose %d = %s", iter->first, t.prettyPrint().c_str());
178 
179  if(this->isSlam2d())
180  {
181  t = (models.at(iter->first)[0].localTransform() * t).inverse();
182  t = iter->second.inverse() * t;
183  iter->second *= t.to3DoF();
184  }
185  else
186  {
187  iter->second = (models.at(iter->first)[0].localTransform() * t).inverse();
188  }
189 
190  ++i;
191  }
192 
193  //update 3D points
194  i=0;
195  for(std::map<int, cv::Point3f>::iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
196  {
197  kter->second = points[i++];
198  }
199 
200  return newPoses;
201 
202 #else
203  UERROR("RTAB-Map is not built with cvsba!");
204  return std::map<int, Transform>();
205 #endif
206 }
207 
208 } /* namespace rtabmap */
OptimizerCVSBA.h
util3d_motion_estimation.h
util3d_correspondences.h
Vector2::y
float y
UTimer.h
UMath.h
Basic mathematics functions.
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:91
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:93
util3d_transforms.h
UConversion.h
Some conversion functions.
ULogger::kInfo
@ kInfo
Definition: ULogger.h:252
UASSERT
#define UASSERT(condition)
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
Eigen::Triplet< double >
UWARN
#define UWARN(...)
rtabmap::OptimizerCVSBA::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: OptimizerCVSBA.cpp:56
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
Graph.h
iter
iterator iter(handle obj)
UStl.h
Wrappers of STL for convenient functions.
UDEBUG
#define UDEBUG(...)
Vector2::x
float x
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:90
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::OptimizerCVSBA::available
static bool available()
Definition: OptimizerCVSBA.cpp:47
UERROR
#define UERROR(...)
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
i
int i
R
R


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