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.linear_solver_type = ceres::ITERATIVE_SCHUR;
242  options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
243  options.max_num_iterations = iterations();
244  options.function_tolerance = this->epsilon();
245  ceres::Solver::Summary summary;
246  UTimer timer;
247  ceres::Solve(options, &problem, &summary);
248  if(ULogger::level() == ULogger::kDebug)
249  {
250  UDEBUG("Ceres Report:");
251  std::cout << summary.FullReport() << '\n';
252  }
253  if(!summary.IsSolutionUsable())
254  {
255  UWARN("ceres: Could not find a usable solution, aborting optimization!");
256  return optimizedPoses;
257  }
258 
259  if(finalError)
260  {
261  *finalError = summary.final_cost;
262  }
263  if(iterationsDone)
264  {
265  *iterationsDone = summary.iterations.size();
266  }
267  UINFO("Ceres optimizing end (%d iterations done, error=%f, time = %f s)", (int)summary.iterations.size(), summary.final_cost, timer.ticks());
268 
269 
270  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
271  {
272  if(iter->first > 0)
273  {
274  if(isSlam2d())
275  {
276  const std::map<int, ceres::examples::Pose2d>::iterator & pter = poses2d.find(iter->first);
277  float roll, pitch, yaw;
278  iter->second.getEulerAngles(roll, pitch, yaw);
279 
280  Transform newPose(pter->second.x, pter->second.y, iter->second.z(), roll, pitch, pter->second.yaw_radians);
281 
282  UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
283  optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
284  }
285  else
286  {
287  const std::map<int, ceres::examples::Pose3d, std::less<int>,
288  Eigen::aligned_allocator<std::pair<const int, ceres::examples::Pose3d> > >::
289  iterator& pter = poses3d.find(iter->first);
290 
291  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());
292 
293  UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
294  optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
295 
296  }
297  }
298  }
299 
300  // Ceres doesn't compute marginals...
301  }
302  else if(poses.size() == 1 || iterations() <= 0)
303  {
304  optimizedPoses = poses;
305  }
306  else
307  {
308  UWARN("This method should be called at least with 1 pose!");
309  }
310  UDEBUG("Optimizing graph...end!");
311 #else
312  UERROR("Not built with Ceres support!");
313 #endif
314  return optimizedPoses;
315 }
316 
317 std::map<int, Transform> OptimizerCeres::optimizeBA(
318  int rootId,
319  const std::map<int, Transform> & posesIn,
320  const std::multimap<int, Link> & links,
321  const std::map<int, std::vector<CameraModel> > & models,
322  std::map<int, cv::Point3f> & points3DMap,
323  const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
324  std::set<int> * outliers)
325 {
326 #ifdef RTABMAP_CERES
327  // run sba optimization
328 
329  std::map<int, Transform> poses(posesIn.lower_bound(1), posesIn.end());
330 
331  ceres::BAProblem baProblem;
332 
333  baProblem.num_cameras_ = poses.size();
334  baProblem.num_points_ = points3DMap.size();
335  baProblem.num_observations_ = 0;
336  for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter=wordReferences.begin();
337  iter!=wordReferences.end();
338  ++iter)
339  {
340  baProblem.num_observations_ += iter->second.size();
341  }
342 
343  baProblem.point_index_ = new int[baProblem.num_observations_];
344  baProblem.camera_index_ = new int[baProblem.num_observations_];
345  baProblem.observations_ = new double[4 * baProblem.num_observations_];
346  baProblem.cameras_ = new double[6 * baProblem.num_cameras_];
347  baProblem.points_ = new double[3 * baProblem.num_points_];
348 
349  // Each camera is a set of 6 parameters: R and t. The rotation R is specified as a Rodrigues' vector.
350  int oi=0;
351  int camIndex=0;
352  std::map<int, int> camIdToIndex;
353  for(std::map<int, Transform>::const_iterator iter=poses.begin();
354  iter!=poses.end();
355  ++iter)
356  {
357  // Get camera model
358  std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
359  UASSERT(iterModel != models.end());
360  if(iterModel->second.size() != 1)
361  {
362  UERROR("Multi-camera BA not implemented for Ceres, only single camera.");
363  return std::map<int, Transform>();
364  }
365  UASSERT(iterModel->second[0].isValidForProjection());
366 
367  const Transform & t = (iter->second * iterModel->second[0].localTransform()).inverse();
368  cv::Mat R = (cv::Mat_<double>(3,3) <<
369  (double)t.r11(), (double)t.r12(), (double)t.r13(),
370  (double)t.r21(), (double)t.r22(), (double)t.r23(),
371  (double)t.r31(), (double)t.r32(), (double)t.r33());
372 
373  cv::Mat rvec(1,3, CV_64FC1);
374  cv::Rodrigues(R, rvec);
375 
376  UASSERT(oi+6 <= baProblem.num_cameras_*6);
377 
378  baProblem.cameras_[oi++] = rvec.at<double>(0,0);
379  baProblem.cameras_[oi++] = rvec.at<double>(0,1);
380  baProblem.cameras_[oi++] = rvec.at<double>(0,2);
381  baProblem.cameras_[oi++] = t.x();
382  baProblem.cameras_[oi++] = t.y();
383  baProblem.cameras_[oi++] = t.z();
384 
385  camIdToIndex.insert(std::make_pair(iter->first, camIndex++));
386  }
387  UASSERT(oi == baProblem.num_cameras_*6);
388 
389  oi=0;
390  int pointIndex=0;
391  std::map<int, int> pointIdToIndex;
392  for(std::map<int, cv::Point3f>::const_iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
393  {
394  UASSERT(oi+3 <= baProblem.num_points_*3);
395 
396  baProblem.points_[oi++] = kter->second.x;
397  baProblem.points_[oi++] = kter->second.y;
398  baProblem.points_[oi++] = kter->second.z;
399 
400  pointIdToIndex.insert(std::make_pair(kter->first, pointIndex++));
401  }
402  UASSERT(oi == baProblem.num_points_*3);
403 
404  oi = 0;
405  for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter=wordReferences.begin();
406  iter!=wordReferences.end();
407  ++iter)
408  {
409  for(std::map<int, FeatureBA>::const_iterator jter=iter->second.begin();
410  jter!=iter->second.end();
411  ++jter)
412  {
413  std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(jter->first);
414  UASSERT(iterModel != models.end());
415  if(iterModel->second.size() != 1)
416  {
417  UERROR("Multi-camera BA not implemented for Ceres, only single camera.");
418  return std::map<int, Transform>();
419  }
420  UASSERT(iterModel->second[0].isValidForProjection());
421 
422  baProblem.camera_index_[oi] = camIdToIndex.at(jter->first);
423  baProblem.point_index_[oi] = pointIdToIndex.at(iter->first);
424  baProblem.observations_[4*oi] = jter->second.kpt.pt.x - iterModel->second[0].cx();
425  baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - iterModel->second[0].cy();
426  baProblem.observations_[4*oi+2] = iterModel->second[0].fx();
427  baProblem.observations_[4*oi+3] = iterModel->second[0].fy();
428  ++oi;
429  }
430  }
431  UASSERT(oi == baProblem.num_observations_);
432 
433  // Build problem
434  const double* observations = baProblem.observations();
435  // Create residuals for each observation in the bundle adjustment problem. The
436  // parameters for cameras and points are added automatically.
437  ceres::Problem problem;
438 
439  for (int i = 0; i < baProblem.num_observations(); ++i) {
440  // Each Residual block takes a point and a camera as input and outputs a 2
441  // dimensional residual. Internally, the cost function stores the observed
442  // image location and compares the reprojection against the observation.
443  ceres::CostFunction* cost_function =
444  ceres::SnavelyReprojectionError::Create(
445  observations[4 * i], //u
446  observations[4 * i + 1], //v
447  observations[4 * i + 2], //fx
448  observations[4 * i + 3]); //fy
449  ceres::LossFunction* loss_function = new ceres::HuberLoss(8.0);
450  problem.AddResidualBlock(cost_function,
451  loss_function,
452  baProblem.mutable_camera_for_observation(i),
453  baProblem.mutable_point_for_observation(i));
454  }
455 
456  // SBA
457  // Make Ceres automatically detect the bundle structure. Note that the
458  // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
459  // for standard bundle adjustment problems.
460  ceres::Solver::Options options;
461  options.linear_solver_type = ceres::ITERATIVE_SCHUR;
462  options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
463  options.max_num_iterations = iterations();
464  //options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
465  options.function_tolerance = this->epsilon();
466  ceres::Solver::Summary summary;
467  ceres::Solve(options, &problem, &summary);
468  if(ULogger::level() == ULogger::kDebug)
469  {
470  UDEBUG("Ceres report:");
471  std::cout << summary.FullReport() << "\n";
472  }
473  if(!summary.IsSolutionUsable())
474  {
475  UWARN("ceres: Could not find a usable solution, aborting optimization!");
476  return poses;
477  }
478 
479  //update poses
480  std::map<int, Transform> newPoses = poses;
481  oi=0;
482  for(std::map<int, Transform>::iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
483  {
484  cv::Mat rvec = (cv::Mat_<double>(1,3) <<
485  baProblem.cameras_[oi], baProblem.cameras_[oi+1], baProblem.cameras_[oi+2]);
486 
487  cv::Mat R;
488  cv::Rodrigues(rvec, R);
489  Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), baProblem.cameras_[oi+3],
490  R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), baProblem.cameras_[oi+4],
491  R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), baProblem.cameras_[oi+5]);
492 
493  oi+=6;
494 
495  if(this->isSlam2d())
496  {
497  t = (models.at(iter->first)[0].localTransform() * t).inverse();
498  t = iter->second.inverse() * t;
499  iter->second *= t.to3DoF();
500  }
501  else
502  {
503  iter->second = (models.at(iter->first)[0].localTransform() * t).inverse();
504  }
505 
506  }
507 
508  //update 3D points
509  oi = 0;
510  for(std::map<int, cv::Point3f>::iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
511  {
512  kter->second.x = baProblem.points_[oi++];
513  kter->second.y = baProblem.points_[oi++];
514  kter->second.z = baProblem.points_[oi++];
515  }
516 
517  return newPoses;
518 
519 #else
520  UERROR("RTAB-Map is not built with ceres!");
521  return std::map<int, Transform>();
522 #endif
523 }
524 
525 } /* namespace rtabmap */
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, 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: OptimizerCeres.cpp:317
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
ceres::examples::NormalizeAngle
T NormalizeAngle(const T &angle_radians)
Definition: normalize_angle.h:57
OptimizerCeres.h
UINFO
#define UINFO(...)
ceres::examples::Pose3d
Definition: pose_graph_3d/types.h:45
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
timer
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
ceres::BAProblem::point_index_
int * point_index_
Definition: BAProblem.h:43
ULogger::kDebug
@ kDebug
Definition: ULogger.h:252
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
ceres::BAProblem::points_
double * points_
Definition: BAProblem.h:47
Vector2::y
float y
UTimer.h
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
ceres::BAProblem::mutable_point_for_observation
double * mutable_point_for_observation(int i)
Definition: BAProblem.h:37
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
ceres::BAProblem::observations_
double * observations_
Definition: BAProblem.h:45
angle_local_parameterization.h
Eigen::aligned_allocator
ceres::BAProblem::num_observations_
int num_observations_
Definition: BAProblem.h:42
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
types.h
UConversion.h
Some conversion functions.
Eigen::PlainObjectBase::data
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar * data()
ceres::examples::Pose2d
Definition: pose_graph_2d/types.h:47
UASSERT
#define UASSERT(condition)
BAProblem.h
ceres::examples::Constraint3d
Definition: pose_graph_3d/types.h:72
p
Point3_ p(2)
ceres::examples::AngleLocalParameterization::Create
static ceres::LocalParameterization * Create()
Definition: angle_local_parameterization.h:54
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
pose_graph_3d_error_term.h
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UWARN
#define UWARN(...)
ceres::EigenQuaternionParameterization
Definition: eigen_quaternion_parameterization.h:49
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
ceres::BAProblem::observations
const double * observations() const
Definition: BAProblem.h:33
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
Graph.h
iter
iterator iter(handle obj)
ceres::BAProblem::num_cameras_
int num_cameras_
Definition: BAProblem.h:40
pose_graph_2d_error_term.h
ceres::BAProblem::num_observations
int num_observations() const
Definition: BAProblem.h:32
UStl.h
Wrappers of STL for convenient functions.
rtabmap::OptimizerCeres::available
static bool available()
Definition: OptimizerCeres.cpp:57
eigen_quaternion_parameterization.h
UDEBUG
#define UDEBUG(...)
UTimer
Definition: UTimer.h:46
ceres::BAProblem::num_points_
int num_points_
Definition: BAProblem.h:41
Eigen::Matrix< double, 3, 3 >
Vector2::x
float x
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:90
NULL
#define NULL
ceres::BAProblem::cameras_
double * cameras_
Definition: BAProblem.h:46
t
Point2 t(10, 10)
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:92
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
ceres::BAProblem
Definition: BAProblem.h:13
options
i
int i
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
ceres::BAProblem::camera_index_
int * camera_index_
Definition: BAProblem.h:44
ceres::BAProblem::mutable_camera_for_observation
double * mutable_camera_for_observation(int i)
Definition: BAProblem.h:34
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
snavely_reprojection_error.h
types.h
R
R

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