Go to the documentation of this file.
23 #include <gtest/gtest.h>
70 const size_t nMapPts = the_map.
size();
74 vector<TObs> observations;
75 observations.resize(nObs);
84 std::vector<std::pair<size_t, float>> idxs;
88 for (
size_t i = 0; i < nObs; i++)
91 the_map.
getPoint(idxs[i].first, gx, gy);
96 observations[i].ID = idxs[i].first;
110 all_correspondences.reserve(nMapPts * nObs);
113 for (
size_t j = 0; j < nObs; j++)
117 match.
other_x = observations[j].x;
118 match.
other_y = observations[j].y;
120 for (
size_t i = 0; i < nMapPts; i++)
124 all_correspondences.push_back(match);
137 params.ransac_maxSetSize =
140 params.ransac_mahalanobisDistanceThreshold =
142 params.ransac_nSimulations = 0;
143 params.ransac_fuseByCorrsMatch =
true;
144 params.ransac_fuseMaxDiffXY = 0.01f;
145 params.ransac_fuseMaxDiffPhi = 0.1_deg;
146 params.ransac_algorithmForLandmarks =
true;
147 params.probability_find_good_model = 0.999999;
148 params.ransac_min_nSimulations =
168 std::abs(solution_pose.
mean.
phi() - GT_pose.
phi()) < 10.0_deg))
170 std::cerr <<
"Solution pose: " << solution_pose.
mean << endl
171 <<
"Ground truth pose: " << GT_pose << endl;
181 for (
int i = 0; i < 3; i++)
Parameters for se2_l2_robust().
TEST(tfest, ransac_data_assoc)
const float ransac_mahalanobisDistanceThreshold
const size_t MINIMUM_RANSAC_ITERS
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
bool ransac_data_assoc_run()
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
double phi() const
Get the phi angle of the 2D pose (in radians)
double x() const
Common members of all points & poses classes.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Functions for estimating the optimal transformation between two frames of references given measuremen...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
#define ASSERT_(f)
Defines an assertion mechanism.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
map< string, CVectorDouble > results
CPose2D mean
The mean value.
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
const float normalizationStd
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
size_t kdTreeRadiusSearch2D(const num_t x0, const num_t y0, const num_t maxRadiusSqr, std::vector< std::pair< size_t, num_t >> &out_indices_dist) const
KD Tree-based search for all the points within a given radius of some 2D point.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
const size_t NUM_OBSERVATIONS_TO_SIMUL
return_t square(const num_t x)
Inline function for the square of a number.
mrpt::vision::TStereoCalibParams params
bool verbose
Show progress messages to std::cout console (default=true)
void setPoint(size_t index, float x, float y, float z)
Changes a given point from map, with Z defaulting to 0 if not provided.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Output placeholder for se2_l2_robust()
bool se2_l2(const mrpt::tfest::TMatchingPairList &in_correspondences, mrpt::math::TPose2D &out_transformation, mrpt::math::CMatrixDouble33 *out_estimateCovariance=nullptr)
Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames.
return_t drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
This base provides a set of functions for maths stuff.
const size_t NUM_MAP_FEATS
A namespace of pseudo-random numbers generators of diferent distributions.
void resize(size_t newLength) override
Resizes all point buffers so they can hold the given number of points: newly created points are set t...
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
void getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
const size_t RANSAC_MINIMUM_INLIERS
bool se2_l2_robust(const mrpt::tfest::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 ...
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020 | |