Go to the documentation of this file.
10 #include <gtest/gtest.h>
23 #include <Eigen/Dense>
71 const CPose2D good_pose(0.820, 0.084, 8.73_deg);
79 sph->setLocation(0, 0, 0);
80 sph->setColor(1, 0, 0);
83 CDisk::Ptr pln = std::make_shared<opengl::CDisk>();
84 pln->setDiskRadius(2);
85 pln->setPose(
CPose3D(0, 0, 0, 0, 5.0_deg, 5.0_deg));
86 pln->setColor(0.8, 0, 0);
90 CDisk::Ptr pln2 = std::make_shared<opengl::CDisk>();
91 pln2->setDiskRadius(2);
92 pln2->setPose(
CPose3D(0, 0, 0, 30.0_deg, -20.0_deg, -2.0_deg));
93 pln2->setColor(0.9, 0, 0);
125 std::make_shared<CGridPlaneXY>(-20, 20, -20, 20, 0, 1);
126 plane1->setColor(0.3f, 0.3f, 0.3f);
127 scene1->insert(plane1);
128 scene2->insert(plane1);
129 scene3->insert(plane1);
133 scene1->insert(world);
137 std::make_shared<CAngularObservationMesh>();
139 std::make_shared<CAngularObservationMesh>();
141 CAngularObservationMesh::trace2DSetOfRays(
143 CAngularObservationMesh::TDoubleRange::CreateFromAperture(
145 CAngularObservationMesh::TDoubleRange::CreateFromAperture(
147 CAngularObservationMesh::trace2DSetOfRays(
149 CAngularObservationMesh::TDoubleRange::CreateFromAperture(
151 CAngularObservationMesh::TDoubleRange::CreateFromAperture(
158 origin1->setScale(0.6f);
159 scene1->insert(origin1);
160 scene2->insert(origin1);
165 origin2->setScale(0.6f);
166 scene1->insert(origin2);
167 scene2->insert(origin2);
173 aom1->generatePointCloud(&M1);
174 aom2->generatePointCloud(&M2);
190 scene2->insert(PTNS1);
191 scene2->insert(PTNS2);
218 <<
"ICP output: mean= " <<
mean << endl
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
mrpt::poses::CPose3DPDF::Ptr Align3D(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3D &grossEst, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt)
void changeCoordinatesReference(const mrpt::poses::CPose2D &b)
Replace each point by (pose compounding operator).
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt)
The method for aligning a pair of metric maps, for SE(2) relative poses.
TConfigParams options
The options employed by the ICP align.
double smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough.
double ALFA
The scale factor for thresholds every time convergence is achieved.
double thresholdDist
Initial threshold distance for two points to become a correspondence.
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
std::shared_ptr< mrpt::opengl ::CGridPlaneXY > Ptr
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
void generateObjects(CSetOfObjects::Ptr &world)
double mean(const CONTAINER &v)
Computes the mean value of a vector.
std::shared_ptr< mrpt::opengl ::CDisk > Ptr
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
This namespace contains representation of robot actions and observations.
The ICP algorithm return information.
CPose3D viewpoint1(-0.3, 0.7, 3, 5.0_deg, 80.0_deg, 3.0_deg)
void align2scans(const TICPAlgorithm icp_method)
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
An RGBA color - floats in the range [0,1].
vector_t asVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
std::shared_ptr< mrpt::opengl ::CAngularObservationMesh > Ptr
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
TRenderOptions renderOptions
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
constexpr double DEG2RAD(const double x)
Degrees to radians
std::shared_ptr< mrpt::opengl ::CSphere > Ptr
const size_t HOW_MANY_YAWS
std::shared_ptr< mrpt::opengl ::COpenGLScene > Ptr
TEST_F(ICPTests, AlignScans_icpClassic)
CSetOfObjects::Ptr CornerXYZ(float scale=1.0)
Returns three arrows representing a X,Y,Z 3D corner.
void example2DRangeScan(mrpt::obs::CObservation2DRangeScan &s, int i=0)
Example 2D lidar scans (form a venerable SICK LMS200).
unsigned int maxIterations
Maximum number of iterations to run.
bool doRANSAC
Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better es...
CPose3D SCAN2_POSE_ERROR(0.15, -0.07, 0.10, -0.03, 0.1, 0.1)
This base provides a set of functions for maths stuff.
std::shared_ptr< CPose3DPDF > Ptr
static void generateObjects(CSetOfObjects::Ptr &world)
mrpt::img::TColorf color
Color of points.
const size_t HOW_MANY_PITCHS
std::shared_ptr< CPosePDF > Ptr
The namespace for 3D scene representation and rendering.
CPose3D viewpoint2(0.5, -0.2, 2.6, -5.0_deg, 100.0_deg, -7.0_deg)
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic).
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020 | |