Main MRPT website > C++ reference for MRPT 1.5.7
scanmatching_backwards_compat.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "tfest-precomp.h" // Precompiled headers
11 
12 #include <mrpt/tfest.h>
13 
14 #define MRPT_SCANMATCHING_SUPRESS_BACKCOMPAT_WARNING
16 
17 #include <mrpt/poses/CPose3D.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::utils;
21 using namespace mrpt::poses;
22 using namespace mrpt::math;
23 using namespace std;
24 
26  const std::vector<double> &input,
27  mrpt::poses::CPose3DQuat &outQuat,
28  bool forceScaleToUnity)
29 {
30  // Convert input data:
31  const size_t nMatches = input.size()/6;
32  ASSERT_EQUAL_(input.size()%6, 0)
33  vector<TPoint3D> inThis(nMatches), inOther(nMatches);
34  for(size_t i = 0; i < nMatches; i++ )
35  {
36  inThis[i].x = input[i*6+0];
37  inThis[i].y = input[i*6+1];
38  inThis[i].z = input[i*6+2];
39 
40  inOther[i].x = input[i*6+3];
41  inOther[i].y = input[i*6+4];
42  inOther[i].z = input[i*6+5];
43  }
44 
45  // Call:
46  double scale=.0;
47  mrpt::tfest::se3_l2(inThis,inOther,outQuat,scale,forceScaleToUnity);
48 
49  return scale;
50 }
51 
52 
53 // Deprecated: Use mrpt::tfest::se3_l2() instead
55  const std::vector<double> &inPoints,
56  std::vector<double> &outVector,
57  bool forceScaleToUnity)
58 {
60  const double scale = backwards_compat_HornMethod(inPoints,outQuat,forceScaleToUnity);
61  // Output:
62  outVector.resize( 7 );
63  for (int i=0;i<7;i++) outVector[i] = outQuat[i];
64  return scale;
65 }
66 
67 // Deprecated: Use mrpt::tfest::se3_l2() instead
69  const std::vector<double> &inPoints,
70  mrpt::poses::CPose3DQuat &outQuat,
71  bool forceScaleToUnity)
72 {
73  return backwards_compat_HornMethod(inPoints,outQuat,forceScaleToUnity);
74 }
75 
76 // Deprecated: Use mrpt::tfest::se3_l2() instead
78  const mrpt::utils::TMatchingPairList &in_correspondences,
79  mrpt::poses::CPose3DQuat &out_transformation,
80  double &out_scale,
81  const bool forceScaleToUnity)
82 {
83  return mrpt::tfest::se3_l2(in_correspondences,out_transformation,out_scale,forceScaleToUnity);
84 }
85 
86 // Deprecated: Use mrpt::tfest::se3_l2() instead
88  const mrpt::utils::TMatchingPairList &in_correspondences,
89  mrpt::poses::CPose3D &out_transformation,
90  double &out_scale,
91  const bool forceScaleToUnity)
92 {
94  const bool ret = mrpt::tfest::se3_l2(in_correspondences,tf,out_scale,forceScaleToUnity);
95  out_transformation = mrpt::poses::CPose3D(tf);
96  return ret;
97 }
98 
99 // Deprecated: Use mrpt::tfest::se3_l2_robust() instead
101  const mrpt::utils::TMatchingPairList &in_correspondences,
102  mrpt::poses::CPose3D &out_transformation,
103  double &out_scale,
104  vector_int &out_inliers_idx,
105  const unsigned int ransac_minSetSize,
106  const unsigned int ransac_nmaxSimulations,
107  const double ransac_maxSetSizePct,
108  const bool forceScaleToUnity)
109 {
111  params.forceScaleToUnity = forceScaleToUnity;
112  params.ransac_maxSetSizePct = ransac_maxSetSizePct;
113  params.ransac_minSetSize = ransac_minSetSize;
114  params.ransac_nmaxSimulations = ransac_nmaxSimulations;
115 
117  const bool ret = mrpt::tfest::se3_l2_robust(in_correspondences,params,results);
118 
119  out_inliers_idx = results.inliers_idx;
120  out_scale = results.scale;
121  out_transformation = CPose3D(results.transformation);
122 
123  return ret;
124 }
125 
126 // Deprecated: Use mrpt::tfest::se2_l2() instead
128  mrpt::utils::TMatchingPairList &in_correspondences,
129  mrpt::poses::CPose2D &out_transformation,
130  mrpt::math::CMatrixDouble33 *out_estimateCovariance)
131 {
132  mrpt::math::TPose2D estPose;
133  const bool ret = mrpt::tfest::se2_l2(in_correspondences,estPose,out_estimateCovariance);
134  out_transformation = CPose2D(estPose);
135  return ret;
136 }
137 
138 // Deprecated: Use mrpt::tfest::se2_l2() instead
140  mrpt::utils::TMatchingPairList &in_correspondences,
141  mrpt::poses::CPosePDFGaussian &out_transformation )
142 {
143  return mrpt::tfest::se2_l2(in_correspondences,out_transformation);
144 }
145 
146 // Deprecated: Use mrpt::tfest::se2_l2_robust() instead
148  mrpt::utils::TMatchingPairList &in_correspondences,
149  mrpt::poses::CPosePDFSOG &out_transformation,
150  float normalizationStd,
151  unsigned int ransac_minSetSize,
152  unsigned int ransac_maxSetSize,
154  unsigned int ransac_nSimulations,
155  mrpt::utils::TMatchingPairList *out_largestSubSet,
156  bool ransac_fuseByCorrsMatch,
157  float ransac_fuseMaxDiffXY,
158  float ransac_fuseMaxDiffPhi,
159  bool ransac_algorithmForLandmarks,
160  double probability_find_good_model,
161  unsigned int ransac_min_nSimulations,
162  const bool verbose,
163  double max_rmse_to_end
164  )
165 {
167  params.ransac_minSetSize = ransac_minSetSize;
168  params.ransac_maxSetSize = ransac_maxSetSize;
169  params.ransac_mahalanobisDistanceThreshold = ransac_mahalanobisDistanceThreshold;
170  params.ransac_nSimulations = ransac_nSimulations;
171  params.ransac_fuseByCorrsMatch = ransac_fuseByCorrsMatch;
172  params.ransac_fuseMaxDiffXY = ransac_fuseMaxDiffXY;
173  params.ransac_fuseMaxDiffPhi = ransac_fuseMaxDiffPhi;
174  params.ransac_algorithmForLandmarks = ransac_algorithmForLandmarks;
175  params.probability_find_good_model = probability_find_good_model;
176  params.ransac_min_nSimulations = ransac_min_nSimulations;
177  params.max_rmse_to_end = max_rmse_to_end;
178  params.verbose = verbose;
179 
181  //const bool ret =
182  mrpt::tfest::se2_l2_robust(in_correspondences,normalizationStd,params,results);
183 
184  out_transformation = results.transformation;
185  if (out_largestSubSet) *out_largestSubSet = results.largestSubSet;
186 }
187 
#define ASSERT_EQUAL_(__A, __B)
bool TFEST_IMPEXP leastSquareErrorRigidTransformation(mrpt::utils::TMatchingPairList &in_correspondences, mrpt::poses::CPose2D &out_transformation, mrpt::math::CMatrixDouble33 *out_estimateCovariance=NULL)
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
const float normalizationStd
void TFEST_IMPEXP robustRigidTransformation(mrpt::utils::TMatchingPairList &in_correspondences, mrpt::poses::CPosePDFSOG &out_transformation, float normalizationStd, unsigned int ransac_minSetSize=3, unsigned int ransac_maxSetSize=20, float ransac_mahalanobisDistanceThreshold=3.0f, unsigned int ransac_nSimulations=0, mrpt::utils::TMatchingPairList *out_largestSubSet=NULL, bool ransac_fuseByCorrsMatch=true, float ransac_fuseMaxDiffXY=0.01f, float ransac_fuseMaxDiffPhi=mrpt::utils::DEG2RAD(0.1f), bool ransac_algorithmForLandmarks=true, double probability_find_good_model=0.999, unsigned int ransac_min_nSimulations=1500, const bool verbose=false, double max_rmse_to_end=0)
bool TFEST_IMPEXP leastSquareErrorRigidTransformation6D(const mrpt::utils::TMatchingPairList &in_correspondences, mrpt::poses::CPose3DQuat &out_transformation, double &out_scale, const bool forceScaleToUnity=false)
bool TFEST_IMPEXP se3_l2_robust(const mrpt::utils::TMatchingPairList &in_correspondences, const TSE3RobustParams &in_params, TSE3RobustResult &out_results)
Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames ...
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:37
double backwards_compat_HornMethod(const std::vector< double > &input, mrpt::poses::CPose3DQuat &outQuat, bool forceScaleToUnity)
Parameters for se3_l2_robust().
Definition: se3.h:60
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:5717
Parameters for se2_l2_robust().
Definition: se2.h:62
mrpt::utils::TMatchingPairList largestSubSet
the largest consensus sub-set
Definition: se2.h:112
const float ransac_mahalanobisDistanceThreshold
STL namespace.
bool TFEST_IMPEXP se2_l2_robust(const mrpt::utils::TMatchingPairList &in_correspondences, const double in_normalizationStd, const TSE2RobustParams &in_ransac_params, TSE2RobustResult &out_results)
Robust least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference ...
mrpt::vector_int inliers_idx
Indexes within the in_correspondences list which corresponds with inliers.
Definition: se3.h:99
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
A list of TMatchingPair.
Definition: TMatchingPair.h:78
bool TFEST_IMPEXP se3_l2(const mrpt::utils::TMatchingPairList &in_correspondences, mrpt::poses::CPose3DQuat &out_transform, double &out_scale, bool forceScaleToUnity=false)
Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames ...
Definition: se3_l2.cpp:201
double scale
The estimated scale of the rigid transformation (should be very close to 1.0)
Definition: se3.h:98
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
GLenum GLenum GLenum input
Definition: glext.h:5716
Lightweight 2D pose.
mrpt::poses::CPosePDFSOG transformation
The output as a set of transformations (sum of Gaussians)
Definition: se2.h:111
std::vector< int32_t > vector_int
Definition: types_simple.h:23
double TFEST_IMPEXP HornMethod(const std::vector< double > &inPoints, std::vector< double > &outQuat, bool forceScaleToUnity=false)
mrpt::poses::CPose3DQuat transformation
The best transformation found.
Definition: se3.h:97
bool TFEST_IMPEXP se2_l2(const mrpt::utils::TMatchingPairList &in_correspondences, mrpt::math::TPose2D &out_transformation, mrpt::math::CMatrixDouble33 *out_estimateCovariance=NULL)
Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames...
Definition: se2_l2.cpp:45
GLenum const GLfloat * params
Definition: glext.h:3514
Output placeholder for se2_l2_robust()
Definition: se2.h:109
Output placeholder for se3_l2_robust()
Definition: se3.h:95
bool TFEST_IMPEXP leastSquareErrorRigidTransformation6DRANSAC(const mrpt::utils::TMatchingPairList &in_correspondences, mrpt::poses::CPose3D &out_transformation, double &out_scale, vector_int &out_inliers_idx, const unsigned int ransac_minSetSize=5, const unsigned int ransac_nmaxSimulations=50, const double ransac_maxSetSizePct=0.7, const bool forceScaleToUnity=false)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019