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