38 #include <mrpt/maps/COccupancyGridMap2D.h> 39 #include <mrpt/maps/CSimplePointsMap.h> 60 #include <mrpt/version.h> 64 #if MRPT_VERSION >= 0x199 65 using namespace mrpt::img;
66 using namespace mrpt::config;
92 string iniSectionName(
"LocalizationExperiment");
97 iniSectionName,
"particles_count", std::vector<int>(1, 0),
103 param_->
map_file = ini_file.read_string(iniSectionName,
"map_file",
"");
107 SCENE3D_FREQ_ = ini_file.read_int(iniSectionName,
"3DSceneFrequency", 10);
109 ini_file.read_bool(iniSectionName,
"3DSceneFollowRobot",
true);
112 ini_file.read_bool(iniSectionName,
"SHOW_PROGRESS_3D_REAL_TIME",
false);
114 iniSectionName,
"SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS", 1);
116 #if !MRPT_HAS_WXWIDGETS 124 CActionRobotMovement2D::mmGaussian;
127 ini_file.read_double(
"DummyOdometryParams",
"minStdXY", 0.04);
129 ini_file.read_double(
"DefaultOdometryParams",
"minStdPHI", 2.0));
133 ini_file.read_bool(iniSectionName,
"init_PDF_mode",
false,
true);
135 ini_file.read_float(iniSectionName,
"init_PDF_min_x", 0,
true);
137 ini_file.read_float(iniSectionName,
"init_PDF_max_x", 0,
true);
139 ini_file.read_float(iniSectionName,
"init_PDF_min_y", 0,
true);
141 ini_file.read_float(iniSectionName,
"init_PDF_max_y", 0,
true);
143 ini_file.read_float(iniSectionName,
"init_PDF_min_phi_deg", -180));
145 ini_file.read_float(iniSectionName,
"init_PDF_max_phi_deg", 180));
151 min_phi < max_phi ? max_phi - min_phi : (max_phi + 2 *
M_PI) - min_phi;
154 p.phi() = min_phi + cov(2, 2) / 2.0;
156 "----------- phi: %4.3f: %4.3f <-> %4.3f, %4.3f\n", p.phi(), min_phi,
188 _configFile,
"KLD_options");
203 log_info(
"init3DDebug");
207 win3D_ = CDisplayWindow3D::Create(
208 "pf-localization - The MRPT project", 1000, 600);
209 win3D_->setCameraZoom(20);
210 win3D_->setCameraAzimuthDeg(-45);
217 #if MRPT_VERSION >= 0x199 235 "The gridmap has %.04fm2 observed area, %u observed cells\n",
239 "Initial PDF: %f particles/m2\n",
242 auto plane = CSetOfObjects::Create();
248 COpenGLScene::Ptr ptr_scene =
win3D_->get3DSceneAndLock();
250 ptr_scene->insert(plane);
252 ptr_scene->enableFollowCamera(
true);
254 win3D_->unlockAccess3DScene();
258 log_info(
" --------------------------- init3DDebug done \n");
268 if (_observations->size() > 0)
270 _observations->getObservationByIndex(0)->timestamp;
272 #if MRPT_VERSION >= 0x199 280 COpenGLScene::Ptr ptr_scene =
win3D_->get3DSceneAndLock();
282 win3D_->setCameraPointingToPoint(meanPose.
x(), meanPose.
y(), 0);
296 "#particles= %7u", static_cast<unsigned int>(
pdf_.
size())),
302 "mean pose (x y phi_deg)= %s", meanPose.
asString().c_str()),
307 CRenderizable::Ptr parts = ptr_scene->getByName(
"particles");
308 if (parts) ptr_scene->removeObject(parts);
311 p->setName(
"particles");
312 ptr_scene->insert(p);
317 CRenderizable::Ptr ellip = ptr_scene->getByName(
"parts_cov");
320 auto o = CEllipsoid::Create();
322 ellip->setName(
"parts_cov");
323 ellip->setColor(1, 0, 0, 0.6);
327 o->set2DsegmentsCount(60);
328 ptr_scene->insert(ellip);
330 ellip->setLocation(meanPose.
x(), meanPose.
y(), 0.05);
331 #if MRPT_VERSION >= 0x199 332 dynamic_cast<CEllipsoid*
>(ellip.get())->setCovMatrix(cov, 2);
340 CRenderizable::Ptr scan_pts = ptr_scene->getByName(
"scan");
343 auto o = CPointCloud::Create();
345 scan_pts->setName(
"scan");
346 scan_pts->setColor(1, 0, 0, 0.9);
347 o->enableColorFromZ(
false);
349 ptr_scene->insert(scan_pts);
355 CPose3D robot_pose_3D(meanPose);
358 _observations->insertObservationsInto(&map);
361 ->loadFromPointsMap(&last_map);
362 dynamic_cast<CPointCloud*
>(scan_pts.get())->setPose(robot_pose_3D);
367 ptr_scene->enableFollowCamera(
true);
370 COpenGLViewport::Ptr view1 = ptr_scene->getViewport(
"main");
372 CCamera& cam = view1->getCamera();
380 win3D_->unlockAccess3DScene();
389 std::this_thread::sleep_for(
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE
TMonteCarloLocalizationParams options
CMultiMetricMap metric_map_
map
void setOrthogonal(bool v=true)
bool SHOW_PROGRESS_3D_REAL_TIME_
virtual ~PFLocalization()
TDrawSampleMotionModel modelSelection
mrpt::gui::CDisplayWindow3D::Ptr win3D_
mrpt::opengl::COpenGLScene scene_
void setFileName(const std::string &fil_path)
int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS_
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
int initial_particle_count_
number of particles for initialization
void setAzimuthDegrees(float ang)
struct OBS_IMPEXP mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel gaussianModel
void insert(const CRenderizablePtr &newObject, const std::string &viewportName=std::string("main"))
float init_PDF_min_x
any cell
void getAs3DObject(OPENGL_SETOFOBJECTSPTR &out_obj) const
mrpt::maps::CMetricMap * metricMap
double effectiveMappedArea
unsigned long effectiveMappedCells
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
double DEG2RAD(const double x)
mrpt::slam::CMonteCarloLocalization2D pdf_
the filter
mrpt::poses::CPosePDFGaussian initial_pose_
initial posed used in initializeFilter()
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE
#define INVALID_TIMESTAMP
CParticleFilter::TParticleFilterOptions m_options
virtual bool waitForMap()
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const MRPT_OVERRIDE
void setPointingAt(float x, float y, float z)
static const bool loadMap(CMultiMetricMap &_metric_map, const CConfigFile &_config_file, const std::string &_map_file="map.simplemap", const std::string &_section_name="metricMap", bool _debug=false)
void configureFilter(const CConfigFile &_configFile)
void show3DDebug(CSensoryFrame::Ptr _observations)
#define ASSERT_FILE_EXISTS_(FIL)
PFLocalization(Parameters *parm)
void dumpToConsole() const
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2DPtr, TListMaps > m_gridMaps
mrpt::bayes::CParticleFilter pf_
common interface for particle filters
std::string BASE_IMPEXP dateTimeLocalToString(const mrpt::system::TTimeStamp t)
CActionRobotMovement2D::TMotionModelOptions motion_model_default_options_
used if there are is not odom
void setZoomDistance(float z)
void asString(std::string &s) const
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
void setElevationDegrees(float ang)