OptimizerCeres.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2019, 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 
29 #include <rtabmap/utilite/ULogger.h>
30 #include <rtabmap/utilite/UStl.h>
31 #include <rtabmap/utilite/UMath.h>
32 #include <rtabmap/utilite/UConversion.h>
33 #include <rtabmap/utilite/UTimer.h>
34 #include <set>
35 
36 #include <rtabmap/core/optimizer/OptimizerCeres.h>
37 
38 #ifdef RTABMAP_CERES
39 #include <ceres/ceres.h>
40 #include <ceres/local_parameterization.h>
41 #include "ceres/pose_graph_2d/types.h"
42 #include "ceres/pose_graph_2d/pose_graph_2d_error_term.h"
43 #include "ceres/pose_graph_2d/angle_local_parameterization.h"
44 #include "ceres/pose_graph_3d/types.h"
45 #include "ceres/pose_graph_3d/pose_graph_3d_error_term.h"
46 #include "ceres/bundle/BAProblem.h"
47 #include "ceres/bundle/snavely_reprojection_error.h"
48 
49 #if not(CERES_VERSION_MAJOR > 1 || (CERES_VERSION_MAJOR == 1 && CERES_VERSION_MINOR >= 12))
50 #include "ceres/pose_graph_3d/eigen_quaternion_parameterization.h"
51 #endif
52 
53 #endif
54 
55 namespace rtabmap {
56 
57 bool OptimizerCeres::available()
58 {
59 #ifdef RTABMAP_CERES
60  return true;
61 #else
62  return false;
63 #endif
64 }
65 
66 std::map<int, Transform> OptimizerCeres::optimize(
67  int rootId,
68  const std::map<int, Transform> & poses,
69  const std::multimap<int, Link> & edgeConstraints,
70  cv::Mat & outputCovariance,
71  std::list<std::map<int, Transform> > * intermediateGraphes, // contains poses after tree init to last one before the end
72  double * finalError,
73  int * iterationsDone)
74 {
75  outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
76  std::map<int, Transform> optimizedPoses;
77 #ifdef RTABMAP_CERES
78  UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
79  if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
80  {
81  //Build problem
82  ceres::Problem problem;
83  std::map<int, ceres::examples::Pose2d> poses2d;
84  ceres::examples::MapOfPoses poses3d;
85 
86  UDEBUG("fill poses to Ceres...");
87  if(isSlam2d())
88  {
89  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
90  {
91  if(iter->first > 0)
92  {
93  UASSERT(!iter->second.isNull());
94  ceres::examples::Pose2d p;
95  p.x = iter->second.x();
96  p.y = iter->second.y();
97  p.yaw_radians = ceres::examples::NormalizeAngle(iter->second.theta());
98  poses2d.insert(std::make_pair(iter->first, p));
99  }
100  }
101  }
102  else
103  {
104  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
105  {
106  if(iter->first > 0)
107  {
108  UASSERT(!iter->second.isNull());
109  ceres::examples::Pose3d p;
110  p.p.x() = iter->second.x();
111  p.p.y() = iter->second.y();
112  p.p.z() = iter->second.z();
113  p.q = iter->second.getQuaterniond();
114  poses3d.insert(std::make_pair(iter->first, p));
115  }
116  }
117 
118  }
119 
120  ceres::LossFunction* loss_function = NULL;
121  ceres::LocalParameterization* angle_local_parameterization = NULL;
122  ceres::LocalParameterization* quaternion_local_parameterization = NULL;
123 
124  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
125  {
126  int id1 = iter->second.from();
127  int id2 = iter->second.to();
128 
129  if(id1 != id2 && id1 > 0 && id2 > 0)
130  {
131  UASSERT(poses.find(id1) != poses.end() && poses.find(id2) != poses.end());
132 
133  if(isSlam2d())
134  {
135  Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
136  if(!isCovarianceIgnored())
137  {
138  information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
139  information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
140  information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
141  information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
142  information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
143  information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
144  information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
145  information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
146  information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
147  }
148 
149  float yaw_radians = ceres::examples::NormalizeAngle(iter->second.transform().theta());
150  const Eigen::Matrix3d sqrt_information = information.llt().matrixL();
151 
152  // Ceres will take ownership of the pointer.
153  ceres::CostFunction* cost_function = ceres::examples::PoseGraph2dErrorTerm::Create(
154  iter->second.transform().x(),
155  iter->second.transform().y(),
156  yaw_radians,
157  sqrt_information);
158 
159  std::map<int, ceres::examples::Pose2d>::iterator pose_begin_iter = poses2d.find(id1);
160  std::map<int, ceres::examples::Pose2d>::iterator pose_end_iter = poses2d.find(id2);
161 
162  problem.AddResidualBlock(
163  cost_function, loss_function,
164  &pose_begin_iter->second.x, &pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians,
165  &pose_end_iter->second.x, &pose_end_iter->second.y, &pose_end_iter->second.yaw_radians);
166 
167  if(angle_local_parameterization == NULL)
168  {
169  angle_local_parameterization = ceres::examples::AngleLocalParameterization::Create();
170  }
171  problem.SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization);
172  problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization);
173  }
174  else
175  {
176  ceres::examples::MapOfPoses::iterator pose_begin_iter = poses3d.find(id1);
177  ceres::examples::MapOfPoses::iterator pose_end_iter = poses3d.find(id2);
178  ceres::examples::Constraint3d constraint;
179  Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
180  if(!isCovarianceIgnored())
181  {
182  memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
183  }
184 
185  ceres::examples::Pose3d t;
186  t.p.x() = iter->second.transform().x();
187  t.p.y() = iter->second.transform().y();
188  t.p.z() = iter->second.transform().z();
189  t.q = iter->second.transform().getQuaterniond();
190 
191  const Eigen::Matrix<double, 6, 6> sqrt_information = information.llt().matrixL();
192  // Ceres will take ownership of the pointer.
193  ceres::CostFunction* cost_function = ceres::examples::PoseGraph3dErrorTerm::Create(t, sqrt_information);
194  problem.AddResidualBlock(cost_function, loss_function,
195  pose_begin_iter->second.p.data(), pose_begin_iter->second.q.coeffs().data(),
196  pose_end_iter->second.p.data(), pose_end_iter->second.q.coeffs().data());
197  if(quaternion_local_parameterization == NULL)
198  {
199  quaternion_local_parameterization = new ceres::EigenQuaternionParameterization;
200  }
201  problem.SetParameterization(pose_begin_iter->second.q.coeffs().data(), quaternion_local_parameterization);
202  problem.SetParameterization(pose_end_iter->second.q.coeffs().data(), quaternion_local_parameterization);
203  }
204  }
205  //else // not supporting pose prior and landmarks
206  }
207 
208  if(isSlam2d())
209  {
210  // The pose graph optimization problem has three DOFs that are not fully
211  // constrained. This is typically referred to as gauge freedom. You can apply
212  // a rigid body transformation to all the nodes and the optimization problem
213  // will still have the exact same cost. The Levenberg-Marquardt algorithm has
214  // internal damping which mitigate this issue, but it is better to properly
215  // constrain the gauge freedom. This can be done by setting one of the poses
216  // as constant so the optimizer cannot change it.
217  std::map<int, ceres::examples::Pose2d>::iterator pose_start_iter = rootId>0?poses2d.find(rootId):poses2d.begin();
218  UASSERT(pose_start_iter != poses2d.end());
219  problem.SetParameterBlockConstant(&pose_start_iter->second.x);
220  problem.SetParameterBlockConstant(&pose_start_iter->second.y);
221  problem.SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
222  }
223  else
224  {
225  // The pose graph optimization problem has six DOFs that are not fully
226  // constrained. This is typically referred to as gauge freedom. You can apply
227  // a rigid body transformation to all the nodes and the optimization problem
228  // will still have the exact same cost. The Levenberg-Marquardt algorithm has
229  // internal damping which mitigates this issue, but it is better to properly
230  // constrain the gauge freedom. This can be done by setting one of the poses
231  // as constant so the optimizer cannot change it.
232  ceres::examples::MapOfPoses::iterator pose_start_iter = rootId>0?poses3d.find(rootId):poses3d.begin();
233  UASSERT(pose_start_iter != poses3d.end());
234  problem.SetParameterBlockConstant(pose_start_iter->second.p.data());
235  problem.SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
236  }
237 
238  UINFO("Ceres optimizing begin (iterations=%d)", iterations());
239 
240  ceres::Solver::Options options;
241  options.max_num_iterations = iterations();
242  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
243  options.function_tolerance = this->epsilon();
244  ceres::Solver::Summary summary;
245  UTimer timer;
246  ceres::Solve(options, &problem, &summary);
247  if(ULogger::level() == ULogger::kDebug)
248  {
249  UDEBUG("Ceres Report:");
250  std::cout << summary.FullReport() << '\n';
251  }
252  if(!summary.IsSolutionUsable())
253  {
254  UWARN("ceres: Could not find a usable solution, aborting optimization!");
255  return optimizedPoses;
256  }
257 
258  if(finalError)
259  {
260  *finalError = summary.final_cost;
261  }
262  if(iterationsDone)
263  {
264  *iterationsDone = summary.iterations.size();
265  }
266  UINFO("Ceres optimizing end (%d iterations done, error=%f, time = %f s)", (int)summary.iterations.size(), summary.final_cost, timer.ticks());
267 
268 
269  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
270  {
271  if(iter->first > 0)
272  {
273  if(isSlam2d())
274  {
275  const std::map<int, ceres::examples::Pose2d>::iterator & pter = poses2d.find(iter->first);
276  float roll, pitch, yaw;
277  iter->second.getEulerAngles(roll, pitch, yaw);
278 
279  Transform newPose(pter->second.x, pter->second.y, iter->second.z(), roll, pitch, pter->second.yaw_radians);
280 
281  UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
282  optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
283  }
284  else
285  {
286  const std::map<int, ceres::examples::Pose3d, std::less<int>,
287  Eigen::aligned_allocator<std::pair<const int, ceres::examples::Pose3d> > >::
288  iterator& pter = poses3d.find(iter->first);
289 
290  Transform newPose(pter->second.p.x(), pter->second.p.y(), pter->second.p.z(), pter->second.q.x(), pter->second.q.y(), pter->second.q.z(), pter->second.q.w());
291 
292  UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
293  optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
294 
295  }
296  }
297  }
298 
299  // Ceres doesn't compute marginals...
300  }
301  else if(poses.size() == 1 || iterations() <= 0)
302  {
303  optimizedPoses = poses;
304  }
305  else
306  {
307  UWARN("This method should be called at least with 1 pose!");
308  }
309  UDEBUG("Optimizing graph...end!");
310 #else
311  UERROR("Not built with Ceres support!");
312 #endif
313  return optimizedPoses;
314 }
315 
316 std::map<int, Transform> OptimizerCeres::optimizeBA(
317  int rootId,
318  const std::map<int, Transform> & posesIn,
319  const std::multimap<int, Link> & links,
320  const std::map<int, CameraModel> & models,
321  std::map<int, cv::Point3f> & points3DMap,
322  const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
323  std::set<int> * outliers)
324 {
325 #ifdef RTABMAP_CERES
326  // run sba optimization
327 
328  std::map<int, Transform> poses(posesIn.lower_bound(1), posesIn.end());
329 
330  ceres::BAProblem baProblem;
331 
332  baProblem.num_cameras_ = poses.size();
333  baProblem.num_points_ = points3DMap.size();
334  baProblem.num_observations_ = 0;
335  for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter=wordReferences.begin();
336  iter!=wordReferences.end();
337  ++iter)
338  {
339  baProblem.num_observations_ += iter->second.size();
340  }
341 
342  baProblem.point_index_ = new int[baProblem.num_observations_];
343  baProblem.camera_index_ = new int[baProblem.num_observations_];
344  baProblem.observations_ = new double[4 * baProblem.num_observations_];
345  baProblem.cameras_ = new double[6 * baProblem.num_cameras_];
346  baProblem.points_ = new double[3 * baProblem.num_points_];
347 
348  // Each camera is a set of 6 parameters: R and t. The rotation R is specified as a Rodrigues' vector.
349  int oi=0;
350  int camIndex=0;
351  std::map<int, int> camIdToIndex;
352  for(std::map<int, Transform>::const_iterator iter=poses.begin();
353  iter!=poses.end();
354  ++iter)
355  {
356  // Get camera model
357  std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
358  UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
359 
360  const Transform & t = (iter->second * iterModel->second.localTransform()).inverse();
361  cv::Mat R = (cv::Mat_<double>(3,3) <<
362  (double)t.r11(), (double)t.r12(), (double)t.r13(),
363  (double)t.r21(), (double)t.r22(), (double)t.r23(),
364  (double)t.r31(), (double)t.r32(), (double)t.r33());
365 
366  cv::Mat rvec(1,3, CV_64FC1);
367  cv::Rodrigues(R, rvec);
368 
369  UASSERT(oi+6 <= baProblem.num_cameras_*6);
370 
371  baProblem.cameras_[oi++] = rvec.at<double>(0,0);
372  baProblem.cameras_[oi++] = rvec.at<double>(0,1);
373  baProblem.cameras_[oi++] = rvec.at<double>(0,2);
374  baProblem.cameras_[oi++] = t.x();
375  baProblem.cameras_[oi++] = t.y();
376  baProblem.cameras_[oi++] = t.z();
377 
378  camIdToIndex.insert(std::make_pair(iter->first, camIndex++));
379  }
380  UASSERT(oi == baProblem.num_cameras_*6);
381 
382  oi=0;
383  int pointIndex=0;
384  std::map<int, int> pointIdToIndex;
385  for(std::map<int, cv::Point3f>::const_iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
386  {
387  UASSERT(oi+3 <= baProblem.num_points_*3);
388 
389  baProblem.points_[oi++] = kter->second.x;
390  baProblem.points_[oi++] = kter->second.y;
391  baProblem.points_[oi++] = kter->second.z;
392 
393  pointIdToIndex.insert(std::make_pair(kter->first, pointIndex++));
394  }
395  UASSERT(oi == baProblem.num_points_*3);
396 
397  oi = 0;
398  for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter=wordReferences.begin();
399  iter!=wordReferences.end();
400  ++iter)
401  {
402  for(std::map<int, FeatureBA>::const_iterator jter=iter->second.begin();
403  jter!=iter->second.end();
404  ++jter)
405  {
406  std::map<int, CameraModel>::const_iterator iterModel = models.find(jter->first);
407  UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
408 
409  baProblem.camera_index_[oi] = camIdToIndex.at(jter->first);
410  baProblem.point_index_[oi] = pointIdToIndex.at(iter->first);
411  baProblem.observations_[4*oi] = jter->second.kpt.pt.x - iterModel->second.cx();
412  baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - iterModel->second.cy();
413  baProblem.observations_[4*oi+2] = iterModel->second.fx();
414  baProblem.observations_[4*oi+3] = iterModel->second.fy();
415  ++oi;
416  }
417  }
418  UASSERT(oi == baProblem.num_observations_);
419 
420  // Build problem
421  const double* observations = baProblem.observations();
422  // Create residuals for each observation in the bundle adjustment problem. The
423  // parameters for cameras and points are added automatically.
424  ceres::Problem problem;
425 
426  for (int i = 0; i < baProblem.num_observations(); ++i) {
427  // Each Residual block takes a point and a camera as input and outputs a 2
428  // dimensional residual. Internally, the cost function stores the observed
429  // image location and compares the reprojection against the observation.
430  ceres::CostFunction* cost_function =
431  ceres::SnavelyReprojectionError::Create(
432  observations[4 * i], //u
433  observations[4 * i + 1], //v
434  observations[4 * i + 2], //fx
435  observations[4 * i + 3]); //fy
436  ceres::LossFunction* loss_function = new ceres::HuberLoss(8.0);
437  problem.AddResidualBlock(cost_function,
438  loss_function,
439  baProblem.mutable_camera_for_observation(i),
440  baProblem.mutable_point_for_observation(i));
441  }
442 
443  // SBA
444  // Make Ceres automatically detect the bundle structure. Note that the
445  // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
446  // for standard bundle adjustment problems.
447  ceres::Solver::Options options;
448  options.linear_solver_type = ceres::DENSE_SCHUR;
449  options.max_num_iterations = iterations();
450  //options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
451  options.function_tolerance = this->epsilon();
452  ceres::Solver::Summary summary;
453  ceres::Solve(options, &problem, &summary);
454  if(ULogger::level() == ULogger::kDebug)
455  {
456  UDEBUG("Ceres report:");
457  std::cout << summary.FullReport() << "\n";
458  }
459  if(!summary.IsSolutionUsable())
460  {
461  UWARN("ceres: Could not find a usable solution, aborting optimization!");
462  return poses;
463  }
464 
465  //update poses
466  std::map<int, Transform> newPoses = poses;
467  oi=0;
468  for(std::map<int, Transform>::iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
469  {
470  cv::Mat rvec = (cv::Mat_<double>(1,3) <<
471  baProblem.cameras_[oi], baProblem.cameras_[oi+1], baProblem.cameras_[oi+2]);
472 
473  cv::Mat R;
474  cv::Rodrigues(rvec, R);
475  Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), baProblem.cameras_[oi+3],
476  R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), baProblem.cameras_[oi+4],
477  R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), baProblem.cameras_[oi+5]);
478 
479  oi+=6;
480 
481  if(this->isSlam2d())
482  {
483  t = (models.at(iter->first).localTransform() * t).inverse();
484  t = iter->second.inverse() * t;
485  iter->second *= t.to3DoF();
486  }
487  else
488  {
489  iter->second = (models.at(iter->first).localTransform() * t).inverse();
490  }
491 
492  }
493 
494  //update 3D points
495  oi = 0;
496  for(std::map<int, cv::Point3f>::iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
497  {
498  kter->second.x = baProblem.points_[oi++];
499  kter->second.y = baProblem.points_[oi++];
500  kter->second.z = baProblem.points_[oi++];
501  }
502 
503  return newPoses;
504 
505 #else
506  UERROR("RTAB-Map is not built with ceres!");
507  return std::map<int, Transform>();
508 #endif
509 }
510 
511 } /* namespace rtabmap */
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
NULL
#define NULL
types.h
UTimer
Definition: UTimer.h:46
ceres::SnavelyReprojectionError::Create
static ceres::CostFunction * Create(const double observed_x, const double observed_y, const double fx, const double fy)
Definition: snavely_reprojection_error.h:54
ceres::examples::PoseGraph2dErrorTerm::Create
static ceres::CostFunction * Create(double x_ab, double y_ab, double yaw_ab_radians, const Eigen::Matrix3d &sqrt_information)
Definition: pose_graph_2d_error_term.h:90
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:92
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:94
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
ceres::examples::NormalizeAngle
T NormalizeAngle(const T &angle_radians)
Definition: normalize_angle.h:57
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
ceres::examples::AngleLocalParameterization::Create
static ceres::LocalParameterization * Create()
Definition: angle_local_parameterization.h:54
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
types.h
rtabmap::OptimizerCeres::optimize
virtual std::map< int, Transform > optimize(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &edgeConstraints, cv::Mat &outputCovariance, std::list< std::map< int, Transform > > *intermediateGraphes=0, double *finalError=0, int *iterationsDone=0)
Definition: OptimizerCeres.cpp:66
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:91
angle_local_parameterization.h
pose_graph_3d_error_term.h
ceres::examples::MapOfPoses
std::map< int, Pose3d, std::less< int >, Eigen::aligned_allocator< std::pair< const int, Pose3d > > > MapOfPoses
Definition: pose_graph_3d/types.h:68
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
pose_graph_2d_error_term.h
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
eigen_quaternion_parameterization.h
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
ceres::examples::PoseGraph3dErrorTerm::Create
static ceres::CostFunction * Create(const Pose3d &t_ab_measured, const Eigen::Matrix< double, 6, 6 > &sqrt_information)
Definition: pose_graph_3d_error_term.h:112
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
snavely_reprojection_error.h
rtabmap::OptimizerCeres::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, CameraModel > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, FeatureBA > > &wordReferences, std::set< int > *outliers=0)
Definition: OptimizerCeres.cpp:316
rtabmap::Transform::to3DoF
Transform to3DoF() const
Definition: Transform.cpp:210
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:93
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
UINFO
#define UINFO(...)

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