mrpt_ekf_slam_2d.cpp
Go to the documentation of this file.
1 /*
2  * File: mrpt_ekf_slam_2d.cpp
3  * Author: Vladislav Tananaev
4  *
5  */
6 
8 #if MRPT_VERSION >= 0x150
9 #include <mrpt_bridge/utils.h>
10 #endif
11 
13 {
14 #if MRPT_VERSION>=0x150
15 #define gausianModel gaussianModel // a typo was fixed in 1.5.0
16 #endif
17 
19  motion_model_default_options_.modelSelection = CActionRobotMovement2D::mmGaussian;
20  motion_model_default_options_.gausianModel.minStdXY = 0.10;
21  motion_model_default_options_.gausianModel.minStdPHI = 2.0;
22 
23  motion_model_options_.modelSelection = CActionRobotMovement2D::mmGaussian;
24  motion_model_options_.gausianModel.a1 = 0.034;
25  motion_model_options_.gausianModel.a2 = 0.057;
26  motion_model_options_.gausianModel.a3 = 0.014;
27  motion_model_options_.gausianModel.a4 = 0.097;
28  motion_model_options_.gausianModel.minStdXY = 0.005;
29  motion_model_options_.gausianModel.minStdPHI = 0.05;
30 
31  // display values
32  SHOW_3D_LIVE = false;
34 }
35 
37 {
38 }
39 
40 void EKFslam::read_iniFile(std::string ini_filename)
41 {
42  CConfigFile iniFile(ini_filename);
43 
44  // Load the config options for mapping:
45  // ----------------------------------------
46  mapping.loadOptions(iniFile);
47  mapping.KF_options.dumpToConsole();
48  mapping.options.dumpToConsole();
49 
50 #if MRPT_VERSION >= 0x150
51  log4cxx::LoggerPtr ros_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
52  mapping.setVerbosityLevel(mrpt_bridge::rosLoggerLvlToMRPTLoggerLvl(ros_logger->getLevel()));
53  mapping.logging_enable_console_output = false;
54 
55 #if MRPT_VERSION >= 0x199
56  mapping.logRegisterCallback(static_cast<output_logger_callback_t>(&mrpt_bridge::mrptToROSLoggerCallback));
57 #else
58  mapping.logRegisterCallback(static_cast<output_logger_callback_t>(&mrpt_bridge::mrptToROSLoggerCallback_mrpt_15));
59 #endif
60 #endif
61 
62  // read display variables
63  SHOW_3D_LIVE = iniFile.read_bool("MappingApplication", "SHOW_3D_LIVE", false);
64  CAMERA_3DSCENE_FOLLOWS_ROBOT = iniFile.read_bool("MappingApplication", "CAMERA_3DSCENE_FOLLOWS_ROBOT", false);
65 }
66 
67 void EKFslam::observation(CSensoryFrame::Ptr _sf, CObservationOdometry::Ptr _odometry)
68 {
69  action = CActionCollection::Create();
70  CActionRobotMovement2D odom_move;
71  odom_move.timestamp = _sf->getObservationByIndex(0)->timestamp;
72 
73  if (_odometry)
74  {
76  {
77  odomLastObservation_ = _odometry->odometry;
78  }
79 
80  mrpt::poses::CPose2D incOdoPose = _odometry->odometry - odomLastObservation_;
81  odomLastObservation_ = _odometry->odometry;
82  odom_move.computeFromOdometry(incOdoPose, motion_model_options_);
83  action->insert(odom_move);
84  }
86  {
87  odom_move.computeFromOdometry(mrpt::poses::CPose2D(0, 0, 0), motion_model_default_options_);
88  action->insert(odom_move);
89  }
90 }
91 
93 {
94 #if MRPT_HAS_WXWIDGETS
95  if (SHOW_3D_LIVE)
96  {
97  win3d = mrpt::gui::CDisplayWindow3D::Create("KF-SLAM live view", 800, 500);
98  }
99 #endif
100 }
101 
103 {
104  // Save 3D view of the filter state:
105  if (SHOW_3D_LIVE && win3d)
106  {
107  mapping.getCurrentState(robotPose_, LMs_, LM_IDs_, fullState_, fullCov_);
108  // Most of this code was copy and pase from ros::amcl
109 
110  // Get the mean robot pose as 3D:
111  const CPose3D robotPoseMean3D = CPose3D(robotPose_.mean);
112 
113  // Build the path:
114  meanPath.push_back(mrpt_bridge::p2t(robotPoseMean3D));
115 
116  // create the scene
117  COpenGLScene::Ptr scene3D = COpenGLScene::Create();
118  opengl::CGridPlaneXY::Ptr grid = opengl::CGridPlaneXY::Create(-1000, 1000, -1000, 1000, 0, 5);
119  grid->setColor(0.4, 0.4, 0.4);
120  scene3D->insert(grid);
121 
122  // Robot path:
123  opengl::CSetOfLines::Ptr linesPath = opengl::CSetOfLines::Create();
124  linesPath->setColor(1, 0, 0);
125  TPose3D init_pose;
126  if (!meanPath.empty())
127  {
128  init_pose = mrpt_bridge::p2t(CPose3D(meanPath[0]));
129  int path_decim = 0;
130  for (vector<TPose3D>::iterator it = meanPath.begin(); it != meanPath.end(); ++it)
131  {
132  linesPath->appendLine(init_pose, *it);
133  init_pose = *it;
134  if (++path_decim > 10)
135  {
136  path_decim = 0;
137  mrpt::opengl::CSetOfObjects::Ptr xyz = mrpt::opengl::stock_objects::CornerXYZSimple(0.3f, 2.0f);
138  xyz->setPose(CPose3D(*it));
139  scene3D->insert(xyz);
140  }
141  }
142  scene3D->insert(linesPath);
143  }
144 
145  // finally a big corner for the latest robot pose:
146  mrpt::opengl::CSetOfObjects::Ptr xyz = mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 2.5);
147  xyz->setPose(robotPoseMean3D);
148  scene3D->insert(xyz);
149 
150  // The camera pointing to the current robot pose:
152  {
153  win3d->setCameraPointingToPoint(robotPoseMean3D.x(), robotPoseMean3D.y(), robotPoseMean3D.z());
154  }
155 
156  // Draw latest data association:
157  const CRangeBearingKFSLAM2D::TDataAssocInfo &da = mapping.getLastDataAssociation();
158  mrpt::opengl::CSetOfLines::Ptr lins = mrpt::opengl::CSetOfLines::Create();
159  lins->setLineWidth(1.2);
160  lins->setColor(1, 1, 1);
161  for (std::map<observation_index_t, prediction_index_t>::const_iterator it = da.results.associations.begin();
162  it != da.results.associations.end(); ++it)
163  {
164  const prediction_index_t idxPred = it->second;
165  // This index must match the internal list of features in the map:
166  CRangeBearingKFSLAM2D::KFArray_FEAT featMean;
167  mapping.getLandmarkMean(idxPred, featMean);
168 
169  TPoint3D featMean3D;
170  landmark_to_3d(featMean, featMean3D);
171  // Line: robot -> landmark:
172  lins->appendLine(robotPoseMean3D.x(), robotPoseMean3D.y(), robotPoseMean3D.z(), featMean3D.x, featMean3D.y,
173  featMean3D.z);
174  }
175  scene3D->insert(lins);
176 
177  // The current state of KF-SLAM:
178  opengl::CSetOfObjects::Ptr objs = opengl::CSetOfObjects::Create();
179  mapping.getAs3DObject(objs);
180  scene3D->insert(objs);
181 
182  mrpt::opengl::COpenGLScene::Ptr &scn = win3d->get3DSceneAndLock();
183  scn = scene3D;
184 
185  win3d->unlockAccess3DScene();
186  win3d->repaint();
187  }
188 }
189 void EKFslam::landmark_to_3d(const CRangeBearingKFSLAM2D::KFArray_FEAT &lm, TPoint3D &p)
190 {
191  p.x = lm[0];
192  p.y = lm[1];
193  p.z = 0;
194 }
bool use_motion_model_default_options_
used default odom_params
static CDisplayWindow3DPtr Create(const std::string &windowCaption, unsigned int initialWindowWidth=400, unsigned int initialWindowHeight=300)
bool SHOW_3D_LIVE
CVectorDouble fullState_
full state vector
EKFslam()
constructor
CMatrixDouble fullCov_
full covariance matrix
static CSetOfLinesPtr Create(const std::vector< mrpt::math::TSegment3D > &sgms, const bool antiAliasing=true)
CPosePDFGaussian robotPose_
current robot pose
void run3Dwindow()
run 3D window update from mrpt lib
mrpt::poses::CPose2D odomLastObservation_
last observation of odometry
std::vector< mrpt::math::TPoint2D > LMs_
vector of the landmarks
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
void init3Dwindow()
init 3D window from mrpt lib
std::map< unsigned int, CLandmark::TLandmarkID > LM_IDs_
vector of the landmarks ID
mrpt::math::TPose3D p2t(const mrpt::poses::CPose3D &p)
CSetOfObjectsPtr OPENGL_IMPEXP CornerXYZSimple(float scale=1.0, float lineWidth=1.0)
CConfigFile * iniFile
VerbosityLevel rosLoggerLvlToMRPTLoggerLvl(log4cxx::LevelPtr lvl)
GLfloat GLfloat p
GLenum GLenum GLenum GLenum mapping
static bool empty()
bool CAMERA_3DSCENE_FOLLOWS_ROBOT
virtual ~EKFslam()
destructor
void mrptToROSLoggerCallback(const std::string &msg, const VerbosityLevel level, const std::string &loggerName, const mrpt::system::TTimeStamp timestamp)
std::map< observation_index_t, prediction_index_t > associations
size_t prediction_index_t
vector< TPose3D > meanPath
void observation(CSensoryFrame::Ptr _sf, CObservationOdometry::Ptr _odometry)
calculate the actions from odometry model for current observation
mrpt::gui::CDisplayWindow3D::Ptr win3d
MRPT window.
void mrptToROSLoggerCallback_mrpt_15(const std::string &msg, const VerbosityLevel level, const std::string &loggerName, const mrpt::system::TTimeStamp timestamp, void *userParam)
CActionRobotMovement2D::TMotionModelOptions motion_model_default_options_
used if there are is not odom
void landmark_to_3d(const CRangeBearingKFSLAM2D::KFArray_FEAT &lm, TPoint3D &p)
convert landmark to 3d point
#define ROSCONSOLE_DEFAULT_NAME
void read_iniFile(std::string ini_filename)
read ini file
CActionRobotMovement2D::TMotionModelOptions motion_model_options_
used with odom value motion noise
CActionCollection::Ptr action
actions
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f


mrpt_ekf_slam_2d
Author(s): Jose Luis, Vladislav Tananaev
autogenerated on Sat May 2 2020 03:43:58