/*---------------------------------------------------------------
loadFromConfigFile
---------------------------------------------------------------*/
void CGridMapAligner::TConfigParams::loadFromConfigFile(
const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
methodSelection =
iniFile.read_enum(section, "methodSelection", methodSelection);
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
featsPerSquareMeter, float, iniFile, section)
MRPT_LOAD_CONFIG_VAR(ransac_SOG_sigma_m, float, iniFile, section)
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(threshold_max, float, iniFile, section)
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(threshold_delta, float, iniFile, section)
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(min_ICP_goodness, float, iniFile, section)
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(max_ICP_mahadist, double, iniFile, section)
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(maxKLd_for_merge, float, iniFile, section)
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
ransac_minSetSizeRatio, float, iniFile, section)
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
ransac_mahalanobisDistanceThreshold, float, iniFile, section)
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
ransac_chi2_quantile, double, iniFile, section)
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
ransac_prob_good_inliers, double, iniFile, section)
MRPT_LOAD_CONFIG_VAR(save_feat_coors, bool, iniFile, section)
MRPT_LOAD_CONFIG_VAR(debug_show_corrs, bool, iniFile, section)
MRPT_LOAD_CONFIG_VAR(debug_save_map_pairs, bool, iniFile, section)
feature_descriptor = iniFile.read_enum(
section, "feature_descriptor", feature_descriptor, true);
feature_detector_options.loadFromConfigFile(iniFile, section);
}
void CRandomFieldGridMap3D::TInsertionOptions::loadFromConfigFile(
const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
GMRF_lambdaPrior = iniFile.read_double(
section.c_str(), "GMRF_lambdaPrior", GMRF_lambdaPrior);
GMRF_skip_variance = iniFile.read_bool(
section.c_str(), "GMRF_skip_variance", GMRF_skip_variance);
}
/*-------------------------------------------------------------
loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CBoardENoses::loadConfig_sensorSpecific(
const mrpt::config::CConfigFileBase& configSource,
const std::string& iniSection)
{
MRPT_START
m_usbSerialNumber =
configSource.read_string(iniSection, "USB_serialname", "", false);
#ifdef _WIN32
m_COM_port =
configSource.read_string(iniSection, "COM_port_WIN", m_COM_port);
#else
m_COM_port =
configSource.read_string(iniSection, "COM_port_LIN", m_COM_port);
#endif
m_COM_baud =
configSource.read_uint64_t(iniSection, "COM_baudRate", m_COM_baud);
configSource.read_vector(
iniSection, "enose_poses_x", vector<float>(0), enose_poses_x, true);
configSource.read_vector(
iniSection, "enose_poses_y", vector<float>(0), enose_poses_y, true);
configSource.read_vector(
iniSection, "enose_poses_z", vector<float>(0), enose_poses_z, true);
configSource.read_vector(
iniSection, "enose_poses_yaw", vector<float>(0), enose_poses_yaw, true);
configSource.read_vector(
iniSection, "enose_poses_pitch", vector<float>(0), enose_poses_pitch,
true);
configSource.read_vector(
iniSection, "enose_poses_roll", vector<float>(0), enose_poses_roll,
true);
ASSERT_(enose_poses_x.size() == enose_poses_y.size());
ASSERT_(enose_poses_x.size() == enose_poses_z.size());
ASSERT_(enose_poses_x.size() == enose_poses_yaw.size());
ASSERT_(enose_poses_x.size() == enose_poses_pitch.size());
ASSERT_(enose_poses_x.size() == enose_poses_roll.size());
// Pass angles to radians:
enose_poses_yaw *= M_PIf / 180.0f;
enose_poses_pitch *= M_PIf / 180.0f;
enose_poses_roll *= M_PIf / 180.0f;
MRPT_END
}
void CNavigatorManualSequence::loadConfigFile(
const mrpt::config::CConfigFileBase& c)
{
const std::string s = "CNavigatorManualSequence";
programmed_orders.clear();
std::vector<std::string> lstKeys;
c.getAllKeys(s, lstKeys);
for (size_t i = 0; i < lstKeys.size(); i++)
{
std::string s = c.read_string(s, lstKeys[i], "", true);
std::vector<std::string> toks;
mrpt::system::tokenize(s, " \t\r\n", toks);
ASSERTMSG_(
toks.size() > 2,
std::string(
"Wrong format while parsing CNavigatorManualSequence "
"cfg file in entry: ") +
lstKeys[i]);
const double t = atof(toks[0].c_str());
TVelCmd krc;
const size_t nComps = toks.size() - 1;
switch (nComps)
{
case 2:
krc.cmd_vel = mrpt::make_aligned_shared<
mrpt::kinematics::CVehicleVelCmd_DiffDriven>();
break;
case 4:
krc.cmd_vel = mrpt::make_aligned_shared<
mrpt::kinematics::CVehicleVelCmd_Holo>();
break;
default:
THROW_EXCEPTION("Expected 2 or 4 velocity components!");
};
for (size_t i = 0; i < nComps; i++)
krc.cmd_vel->setVelCmdElement(i, atof(toks[i + 1].c_str()));
// insert:
programmed_orders[t] = krc;
}
}
void TSlidingWindow::loadFromConfigFile(
const mrpt::config::CConfigFileBase& source, const std::string& section)
{
MRPT_START;
size_t sliding_win_size =
source.read_int(section, "sliding_win_size", 10, false);
this->resizeWindow(sliding_win_size);
MRPT_END;
}
/*-------------------------------------------------------------
loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CEnoseModular::loadConfig_sensorSpecific(
const mrpt::config::CConfigFileBase& configSource,
const std::string& iniSection)
{
MRPT_START
m_usbSerialNumber =
configSource.read_string(iniSection, "USB_serialname", "", false);
#ifdef _WIN32
m_COM_port = configSource.read_string(iniSection, "COM_port_WIN", "COM1");
#else
m_COM_port =
configSource.read_string(iniSection, "COM_port_LIN", m_COM_port);
#endif
m_COM_baud =
configSource.read_uint64_t(iniSection, "COM_baudRate", m_COM_baud);
MRPT_END
}
void CPTG_DiffDrive_C::saveToConfigFile(
mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
{
MRPT_START
const int WN = 25, WV = 30;
CPTG_DiffDrive_CollisionGridBased::saveToConfigFile(cfg, sSection);
cfg.write(
sSection, "K", K, WN, WV,
"K=+1 forward paths; K=-1 for backwards paths.");
MRPT_END
}
void CCascadeClassifierDetection::init(
const mrpt::config::CConfigFileBase& config)
{
#if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM >= 0x200
// load configuration values
m_options.cascadeFileName =
config.read_string("CascadeClassifier", "cascadeFilename", "");
m_options.scaleFactor =
config.read_double("DetectionOptions", "scaleFactor", 1.1);
m_options.minNeighbors =
config.read_int("DetectionOptions", "minNeighbors", 3);
m_options.flags = config.read_int("DetectionOptions", "flags", 0);
m_options.minSize = config.read_int("DetectionOptions", "minSize", 30);
m_cascade = new CascadeClassifier();
// Load cascade classifier from file
CASCADE->load(m_options.cascadeFileName);
// Check if cascade is empty
if (CASCADE->empty()) throw std::runtime_error("Incorrect cascade file.");
#endif
}
/*---------------------------------------------------------------
loadFromConfigFile
---------------------------------------------------------------*/
void CHMTSLAM::TOptions::loadFromConfigFile(
const mrpt::config::CConfigFileBase& source, const std::string& section)
{
MRPT_LOAD_CONFIG_VAR(LOG_OUTPUT_DIR, string, source, section);
MRPT_LOAD_CONFIG_VAR(LOG_FREQUENCY, int, source, section);
MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT(
SLAM_METHOD, int, TLSlamMethod, source, section);
MRPT_LOAD_CONFIG_VAR(SLAM_MIN_DIST_BETWEEN_OBS, float, source, section);
MRPT_LOAD_CONFIG_VAR_DEGREES(SLAM_MIN_HEADING_BETWEEN_OBS, source, section);
MRPT_LOAD_CONFIG_VAR(MIN_ODOMETRY_STD_XY, float, source, section);
MRPT_LOAD_CONFIG_VAR_DEGREES(MIN_ODOMETRY_STD_PHI, source, section);
MRPT_LOAD_CONFIG_VAR(VIEW3D_AREA_SPHERES_HEIGHT, float, source, section);
MRPT_LOAD_CONFIG_VAR(VIEW3D_AREA_SPHERES_RADIUS, float, source, section);
MRPT_LOAD_CONFIG_VAR(random_seed, int, source, section);
stds_Q_no_odo[2] = RAD2DEG(stds_Q_no_odo[2]);
source.read_vector(section, "stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
ASSERT_(stds_Q_no_odo.size() == 3);
stds_Q_no_odo[2] = DEG2RAD(stds_Q_no_odo[2]);
std::string sTLC_detectors =
source.read_string(section, "TLC_detectors", "", true);
mrpt::system::tokenize(sTLC_detectors, ", ", TLC_detectors);
std::cout << "TLC_detectors: " << TLC_detectors.size() << std::endl;
// load other sub-classes:
AA_options.loadFromConfigFile(source, section);
}
void CLMS100Eth::loadConfig_sensorSpecific(
const mrpt::config::CConfigFileBase& configSource,
const std::string& iniSection)
{
C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
float pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll;
pose_x = configSource.read_float(iniSection, "pose_x", 0, false);
pose_y = configSource.read_float(iniSection, "pose_y", 0, false);
pose_z = configSource.read_float(iniSection, "pose_z", 0, false);
pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, false);
pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, false);
pose_roll = configSource.read_float(iniSection, "pose_roll", 0, false);
m_ip = configSource.read_string(
iniSection, "ip_address", "192.168.0.1", false);
m_port = configSource.read_int(iniSection, "TCP_port", 2111, false);
m_process_rate =
configSource.read_int(iniSection, string("process_rate"), 10, false);
m_sensorLabel =
configSource.read_string(iniSection, "sensorLabel", "SICK", false);
m_sensorPose = CPose3D(
pose_x, pose_y, pose_z, DEG2RAD(pose_yaw), DEG2RAD(pose_pitch),
DEG2RAD(pose_roll));
}
/* -----------------------------------------------------
loadConfig_sensorSpecific
----------------------------------------------------- */
void CGillAnemometer::loadConfig_sensorSpecific(
const mrpt::config::CConfigFileBase& configSource,
const std::string& iniSection)
{
#ifdef _WIN32
com_port =
configSource.read_string(iniSection, "COM_port_WIN", "COM1", true);
#else
com_port =
configSource.read_string(iniSection, "COM_port_LIN", "/dev/tty0", true);
#endif
com_bauds = configSource.read_int(iniSection, "COM_baudRate", 9600, false);
pose_x = configSource.read_float(iniSection, "pose_x", 0, true);
pose_y = configSource.read_float(iniSection, "pose_y", 0, true);
pose_z = configSource.read_float(iniSection, "pose_z", 0, true);
pose_roll = configSource.read_float(iniSection, "pose_roll", 0, true);
pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, true);
pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, true);
}
/*-------------------------------------------------------------
loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CIMUXSens::loadConfig_sensorSpecific(
const mrpt::config::CConfigFileBase& configSource,
const std::string& iniSection)
{
m_sensorPose.setFromValues(
configSource.read_float(iniSection, "pose_x", 0, false),
configSource.read_float(iniSection, "pose_y", 0, false),
configSource.read_float(iniSection, "pose_z", 0, false),
DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
m_COMbauds =
configSource.read_int(iniSection, "baudRate", m_COMbauds, false);
#ifdef _WIN32
m_com_port =
configSource.read_string(iniSection, "COM_port_WIN", m_com_port, false);
#else
m_com_port =
configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false);
#endif
}
/* -----------------------------------------------------
loadConfig_sensorSpecific
----------------------------------------------------- */
void CPhidgetInterfaceKitProximitySensors::loadConfig_sensorSpecific(
const mrpt::config::CConfigFileBase& configSource,
const std::string& iniSection)
{
#if MRPT_HAS_PHIDGET
if (!configSource.sectionExists(iniSection))
THROW_EXCEPTION("Can't find section in configuration file");
// looking for the board parameters.
// process_rate = 100 // Hz (common to all sensors)
// serialNumber = 12345 // The interface kit serial
// number.
m_process_rate =
configSource.read_int(iniSection, string("process_rate"), 50);
m_serialNumber =
configSource.read_int(iniSection, string("serialNumber"), -1);
bool display = configSource.read_bool(
iniSection, string("displayRecapitulativeInformations"), false);
// Looking for each sensor.
for (int i = 1; i <= 8; i++)
{
string sensorNKeyName = format("sensor%d", i);
string sensorType = configSource.read_string(
iniSection, sensorNKeyName, string("UNPLUGGED"));
if (sensorType != string("UNPLUGGED"))
{
// the sensor is plugged :
// // check if the sensor type is supported.
if (sensorType == string("EZ1"))
{
m_sensorType[i - 1] = EZ1;
m_minRange[i - 1] = 0.15; // meters
m_maxRange[i - 1] = 6.45; // meters
}
else if (sensorType == string("SHARP-30cm"))
{
m_sensorType[i - 1] = SHARP_30cm;
m_minRange[i - 1] = 0.04; // meters
m_maxRange[i - 1] = 0.3; // meters
}
else if (sensorType == string("SHARP-80cm"))
{
m_sensorType[i - 1] = SHARP_80cm;
m_minRange[i - 1] = 0.06; // meters
m_maxRange[i - 1] = 0.8; // meters
}
else
{
string err = format("Type of sensor %d is not supported", i);
m_state = CGenericSensor::ssError;
THROW_EXCEPTION(err);
}
m_sensorIsPlugged[i - 1] = true;
// reading the sensor pose.
string sensorNPoseX = format("pose%d_x", i);
string sensorNPoseY = format("pose%d_y", i);
string sensorNPoseZ = format("pose%d_z", i);
string sensorNPoseYaw = format("pose%d_yaw", i);
string sensorNPosePitch = format("pose%d_pitch", i);
string sensorNPoseRoll = format("pose%d_roll", i);
float x = configSource.read_float(iniSection, sensorNPoseX, 0.0);
float y = configSource.read_float(iniSection, sensorNPoseY, 0.0);
float z = configSource.read_float(iniSection, sensorNPoseZ, 0.0);
float yaw =
configSource.read_float(iniSection, sensorNPoseYaw, 0.0);
float pitch =
configSource.read_float(iniSection, sensorNPosePitch, 0.0);
float roll =
configSource.read_float(iniSection, sensorNPoseRoll, 0.0);
m_sensorPoses[i - 1] =
mrpt::poses::CPose3D(x, y, z, yaw, pitch, roll);
}
}
if (display)
{ // width = 80;
cout.fill(' ');
cout << "+-------------------------------------------------------------"
"-----------------+"
<< endl;
cout.width(79);
cout << "| Phidget interfaceKit board number : " << m_serialNumber;
cout << "|" << endl;
cout << "| Process rate : " << m_process_rate;
cout << "|" << endl;
cout << "+---------+---------------------+-----------------------------"
"-----------------+"
<< endl;
cout << "| # + Sensor type | Sensor 3D pose "
" |"
<< endl;
cout << "+---------+---------------------+-----------------------------"
"-----------------+"
<< endl;
for (int i = 0; i < 8; i++)
{
cout << "|";
cout.width(9);
cout << i + 1;
cout << " |";
cout.width(19);
switch (m_sensorType[i])
{
case EZ1:
cout << "EZ1 |";
break;
case SHARP_30cm:
cout << "SHARP_30cm |";
break;
case SHARP_80cm:
cout << "SHARP_80cm |";
break;
case UNPLUGGED:
cout << "UNPLUGGED |";
break;
}
cout.width(43);
cout << m_sensorPoses[i];
cout << "|" << endl;
}
cout << "+-------------------------------------------------------------"
"-----------------+"
<< endl;
}
#else
MRPT_UNUSED_PARAM(configSource);
MRPT_UNUSED_PARAM(iniSection);
#endif
}
/* -----------------------------------------------------
loadConfig_sensorSpecific
----------------------------------------------------- */
void CGPSInterface::loadConfig_sensorSpecific(
const mrpt::config::CConfigFileBase& configSource,
const std::string& iniSection)
{
m_parser = configSource.read_enum<CGPSInterface::PARSERS>(
iniSection, "parser", m_parser, false /*Allow default values*/);
m_raw_dump_file_prefix = configSource.read_string(
iniSection, "raw_dump_file_prefix", m_raw_dump_file_prefix,
false /*Allow default values*/);
#ifdef _WIN32
m_COMname =
configSource.read_string(iniSection, "COM_port_WIN", m_COMname, true);
#else
m_COMname =
configSource.read_string(iniSection, "COM_port_LIN", m_COMname, true);
#endif
m_COMbauds =
configSource.read_int(iniSection, "baudRate", m_COMbauds, true);
m_sensorLabelAppendMsgType = configSource.read_bool(
iniSection, "sensor_label_append_msg_type", m_sensorLabelAppendMsgType);
// legacy custom cmds:
m_customInit =
configSource.read_string(iniSection, "customInit", m_customInit, false);
// new custom cmds:
m_custom_cmds_delay = configSource.read_float(
iniSection, "custom_cmds_delay", m_custom_cmds_delay);
m_custom_cmds_append_CRLF = configSource.read_bool(
iniSection, "custom_cmds_append_CRLF", m_custom_cmds_append_CRLF);
// Load as many strings as found on the way:
m_setup_cmds.clear();
for (int i = 1; true; i++)
{
std::string sLine = configSource.read_string(
iniSection, mrpt::format("setup_cmd%i", i), std::string());
sLine = mrpt::system::trim(sLine);
if (sLine.empty()) break;
m_setup_cmds.push_back(sLine);
}
m_shutdown_cmds.clear();
for (int i = 1; true; i++)
{
std::string sLine = configSource.read_string(
iniSection, mrpt::format("shutdown_cmd%i", i), std::string());
sLine = mrpt::system::trim(sLine);
if (sLine.empty()) break;
m_shutdown_cmds.push_back(sLine);
}
m_sensorPose.setFromValues(
configSource.read_float(iniSection, "pose_x", 0),
configSource.read_float(iniSection, "pose_y", 0),
configSource.read_float(iniSection, "pose_z", 0),
DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
m_JAVAD_rtk_src_port = configSource.read_string(
iniSection, "JAVAD_rtk_src_port", m_JAVAD_rtk_src_port);
m_JAVAD_rtk_src_baud = configSource.read_int(
iniSection, "JAVAD_rtk_src_baud", m_JAVAD_rtk_src_baud);
m_JAVAD_rtk_format = configSource.read_string(
iniSection, "JAVAD_rtk_format", m_JAVAD_rtk_format);
m_topcon_useAIMMode = configSource.read_bool(
iniSection, "JAVAD_useAIMMode", m_topcon_useAIMMode);
m_topcon_data_period =
1.0 / configSource.read_double(
iniSection, "outputRate", m_topcon_data_period);
}
/*-------------------------------------------------------------
loadConfig_sensorSpecific
-------------------------------------------------------------*/
void CHokuyoURG::loadConfig_sensorSpecific(
const mrpt::config::CConfigFileBase& configSource,
const std::string& iniSection)
{
m_reduced_fov =
DEG2RAD(configSource.read_float(iniSection, "reduced_fov", 0)),
m_motorSpeed_rpm =
configSource.read_int(iniSection, "HOKUYO_motorSpeed_rpm", 0);
m_sensorPose.setFromValues(
configSource.read_float(iniSection, "pose_x", 0),
configSource.read_float(iniSection, "pose_y", 0),
configSource.read_float(iniSection, "pose_z", 0),
DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
m_highSensMode =
configSource.read_bool(iniSection, "HOKUYO_HS_mode", m_highSensMode);
#ifdef _WIN32
m_com_port =
configSource.read_string(iniSection, "COM_port_WIN", m_com_port);
#else
m_com_port =
configSource.read_string(iniSection, "COM_port_LIN", m_com_port);
#endif
m_ip_dir = configSource.read_string(iniSection, "IP_DIR", m_ip_dir);
m_port_dir = configSource.read_int(iniSection, "PORT_DIR", m_port_dir);
ASSERTMSG_(
!m_com_port.empty() || !m_ip_dir.empty(),
"Either COM_port or IP_DIR must be defined in the configuration file!");
ASSERTMSG_(
m_com_port.empty() || m_ip_dir.empty(),
"Both COM_port and IP_DIR set! Please, define only one of them.");
if (!m_ip_dir.empty())
{
ASSERTMSG_(
m_port_dir,
"A TCP/IP port number `PORT_DIR` must be specified for Ethernet "
"connection");
}
m_disable_firmware_timestamp = configSource.read_bool(
iniSection, "disable_firmware_timestamp", m_disable_firmware_timestamp);
m_intensity = configSource.read_bool(iniSection, "intensity", m_intensity),
MRPT_LOAD_HERE_CONFIG_VAR(
scan_interval, int, m_scan_interval, configSource, iniSection);
// Parent options:
C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
}
void CReactiveNavigationSystem3D::loadConfigFile(
const mrpt::config::CConfigFileBase& c)
{
MRPT_START
m_PTGsMustBeReInitialized = true;
// 1st: load my own params; at the end, call parent's overriden method:
const std::string s = "CReactiveNavigationSystem3D";
unsigned int num_levels;
vector<float> xaux, yaux;
// Read config params which describe the robot shape
num_levels = c.read_int(s, "HEIGHT_LEVELS", 1, true);
m_robotShape.resize(num_levels);
for (unsigned int i = 1; i <= num_levels; i++)
{
m_robotShape.setHeight(
i - 1, c.read_float(s, format("LEVEL%d_HEIGHT", i), 1.0, true));
m_robotShape.setRadius(
i - 1, c.read_float(s, format("LEVEL%d_RADIUS", i), 0.5, false));
c.read_vector(
s, format("LEVEL%d_VECTORX", i), vector<float>(0), xaux, false);
c.read_vector(
s, format("LEVEL%d_VECTORY", i), vector<float>(0), yaux, false);
ASSERT_(xaux.size() == yaux.size());
for (unsigned int j = 0; j < xaux.size(); j++)
m_robotShape.polygon(i - 1).AddVertex(xaux[j], yaux[j]);
}
// Load PTGs from file:
// ---------------------------------------------
// levels = m_robotShape.heights.size()
unsigned int num_ptgs = c.read_int(s, "PTG_COUNT", 1, true);
m_ptgmultilevel.resize(num_ptgs);
// Read each PTG parameters, and generate K x N collisiongrids
// K - Number of PTGs
// N - Number of height sections
for (unsigned int j = 1; j <= num_ptgs; j++)
{
for (unsigned int i = 1; i <= m_robotShape.size(); i++)
{
MRPT_LOG_INFO_FMT(
"[loadConfigFile] Generating PTG#%u at level %u...", j, i);
const std::string sPTGName =
c.read_string(s, format("PTG%d_TYPE", j), "", true);
CParameterizedTrajectoryGenerator* ptgaux =
CParameterizedTrajectoryGenerator::CreatePTG(
sPTGName, c, s, format("PTG%d_", j));
m_ptgmultilevel[j - 1].PTGs.push_back(ptgaux);
}
}
MRPT_LOG_DEBUG_FMT(
" Robot height sections = %u\n",
static_cast<unsigned int>(m_robotShape.size()));
CAbstractPTGBasedReactive::loadConfigFile(
c); // call parent's overriden method:
MRPT_END
}
/** Loads specific configuration for the device from a given source of
* configuration parameters, for example, an ".ini" file, loading from the
* section "[iniSection]" (see config::CConfigFileBase and derived classes)
* \exception This method must throw an exception with a descriptive message if
* some critical parameter is missing or has an invalid value.
*/
void CKinect::loadConfig_sensorSpecific(
const mrpt::config::CConfigFileBase& configSource,
const std::string& iniSection)
{
m_sensorPoseOnRobot.setFromValues(
configSource.read_float(iniSection, "pose_x", 0),
configSource.read_float(iniSection, "pose_y", 0),
configSource.read_float(iniSection, "pose_z", 0),
DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
m_preview_window =
configSource.read_bool(iniSection, "preview_window", m_preview_window);
// "Stereo" calibration data:
// [<SECTION>_LEFT] // Depth
// ...
// [<SECTION>_RIGHT] // RGB
// ...
// [<SECTION>_LEFT2RIGHT_POSE]
// pose_quaternion = [x y z qr qx qy qz]
const mrpt::poses::CPose3D twist(
0, 0, 0, DEG2RAD(-90), DEG2RAD(0), DEG2RAD(-90));
mrpt::img::TStereoCamera sc;
sc.leftCamera = m_cameraParamsDepth; // Load default values so that if we
// fail to load from cfg at least we
// have some reasonable numbers.
sc.rightCamera = m_cameraParamsRGB;
sc.rightCameraPose =
mrpt::poses::CPose3DQuat(m_relativePoseIntensityWRTDepth - twist)
.asTPose();
try
{
sc.loadFromConfigFile(iniSection, configSource);
}
catch (std::exception& e)
{
std::cout << "[CKinect::loadConfig_sensorSpecific] Warning: Ignoring "
"error loading calibration parameters:\n"
<< e.what();
}
m_cameraParamsDepth = sc.leftCamera;
m_cameraParamsRGB = sc.rightCamera;
m_relativePoseIntensityWRTDepth =
twist + mrpt::poses::CPose3D(sc.rightCameraPose);
// Id:
m_user_device_number = configSource.read_int(
iniSection, "device_number", m_user_device_number);
m_grab_image =
configSource.read_bool(iniSection, "grab_image", m_grab_image);
m_grab_depth =
configSource.read_bool(iniSection, "grab_depth", m_grab_depth);
m_grab_3D_points =
configSource.read_bool(iniSection, "grab_3D_points", m_grab_3D_points);
m_grab_IMU = configSource.read_bool(iniSection, "grab_IMU", m_grab_IMU);
m_video_channel = configSource.read_enum<TVideoChannel>(
iniSection, "video_channel", m_video_channel);
{
std::string s = configSource.read_string(
iniSection, "relativePoseIntensityWRTDepth", "");
if (!s.empty()) m_relativePoseIntensityWRTDepth.fromString(s);
}
m_initial_tilt_angle = configSource.read_int(
iniSection, "initial_tilt_angle", m_initial_tilt_angle);
}