9 #if MRPT_VERSION >= 0x150 10 #include <mrpt_bridge/utils.h> 15 #if MRPT_VERSION >= 0x150 16 motion_model_options_.modelSelection = CActionRobotMovement3D::mm6DOF;
18 motion_model_options_.mm6DOFModel.nParticlesCount = 100;
19 motion_model_options_.mm6DOFModel.a1 = 0.114;
20 motion_model_options_.mm6DOFModel.a2 = 0.1114;
21 motion_model_options_.mm6DOFModel.a3 = 0.114;
22 motion_model_options_.mm6DOFModel.a4 = 0.114;
23 motion_model_options_.mm6DOFModel.a5 = 0.13;
24 motion_model_options_.mm6DOFModel.a6 = 0.13;
25 motion_model_options_.mm6DOFModel.a7 = 0.03;
26 motion_model_options_.mm6DOFModel.a8 = 0.03;
27 motion_model_options_.mm6DOFModel.a9 = 0.03;
28 motion_model_options_.mm6DOFModel.a10 = 0.03;
29 motion_model_options_.mm6DOFModel.additional_std_XYZ = 0.005;
30 motion_model_options_.mm6DOFModel.additional_std_angle = 0.0005;
44 CConfigFile iniFile(ini_filename);
49 mapping.KF_options.dumpToConsole();
50 mapping.options.dumpToConsole();
52 #if MRPT_VERSION < 0x150 56 mapping.setVerbosityLevel(mrpt_bridge::rosLoggerLvlToMRPTLoggerLvl(ros_logger->getLevel()));
57 mapping.logging_enable_console_output =
false;
58 #if MRPT_VERSION < 0x199 59 mapping.logRegisterCallback(static_cast<output_logger_callback_t>(&mrpt_bridge::mrptToROSLoggerCallback_mrpt_15));
61 mapping.logRegisterCallback(static_cast<output_logger_callback_t>(&mrpt_bridge::mrptToROSLoggerCallback));
66 SHOW_3D_LIVE = iniFile.read_bool(
"MappingApplication",
"SHOW_3D_LIVE",
false);
72 action = CActionCollection::Create();
74 CActionRobotMovement3D odom_move;
76 odom_move.timestamp = _sf->getObservationByIndex(0)->timestamp;
84 mrpt::poses::CPose3D incOdoPose;
87 #if MRPT_VERSION >= 0x150 88 odom_move.computeFromOdometry(incOdoPose, motion_model_options_);
90 ROS_WARN(
"Ignoring odometry. This feature requires MRPT>=1.5.0.");
98 #if MRPT_HAS_WXWIDGETS 101 win3d = mrpt::gui::CDisplayWindow3D::Create(
"KF-SLAM live view", 800, 500);
115 const CPose3D robotPoseMean3D = CPose3D(
robotPose_.mean);
118 meanPath.push_back(mrpt_bridge::p2t(robotPoseMean3D));
121 COpenGLScene::Ptr scene3D = COpenGLScene::Create();
122 opengl::CGridPlaneXY::Ptr grid = opengl::CGridPlaneXY::Create(-1000, 1000, -1000, 1000, 0, 5);
123 grid->setColor(0.4, 0.4, 0.4);
124 scene3D->insert(grid);
127 opengl::CSetOfLines::Ptr linesPath = opengl::CSetOfLines::Create();
128 linesPath->setColor(1, 0, 0);
132 init_pose = mrpt_bridge::p2t(CPose3D(
meanPath[0]));
134 for (vector<TPose3D>::iterator it =
meanPath.begin(); it !=
meanPath.end(); ++it)
136 linesPath->appendLine(init_pose, *it);
138 if (++path_decim > 10)
141 mrpt::opengl::CSetOfObjects::Ptr xyz = mrpt::opengl::stock_objects::CornerXYZSimple(0.3
f, 2.0
f);
142 xyz->setPose(CPose3D(*it));
143 scene3D->insert(xyz);
146 scene3D->insert(linesPath);
150 mrpt::opengl::CSetOfObjects::Ptr xyz = mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 2.5);
151 xyz->setPose(robotPoseMean3D);
152 scene3D->insert(xyz);
157 win3d->setCameraPointingToPoint(robotPoseMean3D.x(), robotPoseMean3D.y(), robotPoseMean3D.z());
161 const CRangeBearingKFSLAM::TDataAssocInfo &da =
mapping.getLastDataAssociation();
162 mrpt::opengl::CSetOfLines::Ptr lins = mrpt::opengl::CSetOfLines::Create();
163 lins->setLineWidth(1.2);
164 lins->setColor(1, 1, 1);
165 for (std::map<observation_index_t, prediction_index_t>::const_iterator it = da.results.associations.begin();
166 it != da.results.associations.end(); ++it)
168 const prediction_index_t idxPred = it->second;
170 CRangeBearingKFSLAM::KFArray_FEAT featMean;
171 mapping.getLandmarkMean(idxPred, featMean);
176 lins->appendLine(robotPoseMean3D.x(), robotPoseMean3D.y(), robotPoseMean3D.z(), featMean3D.x, featMean3D.y,
179 scene3D->insert(lins);
182 opengl::CSetOfObjects::Ptr objs = opengl::CSetOfObjects::Create();
184 scene3D->insert(objs);
186 mrpt::opengl::COpenGLScene::Ptr &scn =
win3d->get3DSceneAndLock();
189 win3d->unlockAccess3DScene();
CVectorDouble fullState_
full state vector
CMatrixDouble fullCov_
full covariance matrix
std::vector< mrpt::math::TPoint3D > LMs_
vector of the landmarks
CPose3DQuatPDFGaussian robotPose_
current robot pose
void run3Dwindow()
run 3D window update from mrpt lib
void init3Dwindow()
init 3D window from mrpt lib
std::map< unsigned int, CLandmark::TLandmarkID > LM_IDs_
vector of the landmarks ID
void landmark_to_3d(const CRangeBearingKFSLAM::KFArray_FEAT &lm, TPoint3D &p)
convert landmark to 3d point
bool CAMERA_3DSCENE_FOLLOWS_ROBOT
virtual ~EKFslam()
destructor
mrpt::poses::CPose3D odomLastObservation_
last observation of odometry
vector< TPose3D > meanPath
CRangeBearingKFSLAM mapping
EKF slam 3d class.
void observation(CSensoryFrame::Ptr _sf, CObservationOdometry::Ptr _odometry)
calculate the actions from odometry model for current observation
mrpt::gui::CDisplayWindow3D::Ptr win3d
MRPT window.
#define ROSCONSOLE_DEFAULT_NAME
void read_iniFile(std::string ini_filename)
read ini file
CActionCollection::Ptr action
actions