/**
* @brief set configuration parameter
* @param [out] dest : configuration parameter declaration
* @param [int] src : configuration parameter
*/
inline
int proc_conf_set(gnd::conf::configuration *dest, proc_configuration* src) {
gnd_assert(!src, -1, "invalid null pointer");
gnd_assert(!dest, -1, "invalid null pointer");
gnd::conf::set_parameter(dest, &src->kfile);
gnd::conf::set_parameter(dest, &src->output_ssmname);
gnd::conf::set_parameter(dest, &src->output_ssmid);
gnd::conf::set_parameter(dest, &src->motor_ssmname);
gnd::conf::set_parameter(dest, &src->motor_ssmid);
gnd::conf::set_parameter(dest, &src->odmerr_name);
gnd::conf::set_parameter(dest, &src->odmerr_id);
gnd::conf::set_parameter(dest, &src->ad_ssmname);
gnd::conf::set_parameter(dest, &src->ad_ssmid);
gnd::conf::set_parameter(dest, &src->ad_bits);
gnd::conf::set_parameter(dest, &src->ratio_port);
gnd::conf::set_parameter(dest, &src->voltage);
gnd::conf::set_parameter(dest, &src->bias);
gnd::conf::set_parameter(dest, &src->scale_factor);
gnd::conf::get_parameter(dest, &src->spur_adjust);
gnd::conf::set_parameter(dest, &src->gyrodometry);
gnd::conf::set_parameter(dest, &src->error_simulation);
gnd::conf::set_parameter(dest, &src->error_distributuion);
gnd::conf::set_parameter(dest, &src->debug);
return 0;
}
/**
* @brief get configuration parameter
* @param [in] src : configuration parameter declaration
* @param [out] dest : configuration parameter
*/
inline
int proc_conf_get(gnd::conf::configuration *src, proc_configuration* dest) {
gnd_assert(!src, -1, "invalid null pointer");
gnd_assert(!dest, -1, "invalid null pointer");
gnd::conf::get_parameter(src, &dest->bmp_map);
gnd::conf::get_parameter(src, &dest->raw_map);
gnd::conf::get_parameter(src, &dest->sokuikiraw_name);
gnd::conf::get_parameter(src, &dest->sokuikiraw_id);
gnd::conf::get_parameter(src, &dest->sokuikifs_name);
gnd::conf::get_parameter(src, &dest->sokuikifs_id);
gnd::conf::get_parameter(src, &dest->particle_name);
gnd::conf::get_parameter(src, &dest->particle_id);
gnd::conf::get_parameter(src, &dest->odometry_name);
gnd::conf::get_parameter(src, &dest->odometry_id);
gnd::conf::get_parameter(src, &dest->eval_name);
gnd::conf::get_parameter(src, &dest->eval_id);
gnd::conf::get_parameter(src, &dest->cycle);
gnd::conf::get_parameter(src, &dest->sleeping_time);
gnd::conf::get_parameter(src, &dest->sleeping_dist);
gnd::conf::get_parameter(src, &dest->cull);
gnd::conf::get_parameter(src, &dest->blur);
gnd::conf::get_parameter(src, &dest->scan_range);
gnd::conf::get_parameter(src, &dest->matching_false);
gnd::conf::get_parameter(src, &dest->false_positive);
if( gnd::conf::get_parameter(src, &dest->sleeping_orient) >= 0 ){
// convert unit of angle(deg2rad)
dest->sleeping_orient.value = gnd_deg2rad(dest->sleeping_orient.value);
}
return 0;
}
/**
* @brief set configuration parameter declaration
* @param [out] dest : configuration parameter declaration
* @param [in] src : configuration parameter
*/
inline
int proc_conf_set(gnd::conf::configuration *dest, proc_configuration* src) {
gnd_assert(!src, -1, "invalid null pointer");
gnd_assert(!dest, -1, "invalid null pointer");
gnd::conf::set_parameter(dest, &src->bmp_map);
gnd::conf::set_parameter(dest, &src->raw_map);
gnd::conf::set_parameter(dest, &src->sokuikiraw_name);
gnd::conf::set_parameter(dest, &src->sokuikiraw_id);
gnd::conf::set_parameter(dest, &src->odometry_name);
gnd::conf::set_parameter(dest, &src->odometry_id);
gnd::conf::set_parameter(dest, &src->particle_name);
gnd::conf::set_parameter(dest, &src->particle_id);
gnd::conf::set_parameter(dest, &src->eval_name);
gnd::conf::set_parameter(dest, &src->eval_id);
gnd::conf::set_parameter(dest, &src->cycle);
gnd::conf::set_parameter(dest, &src->sleeping_time);
gnd::conf::set_parameter(dest, &src->sleeping_dist);
// convert unit of angle (rad2deg)
src->sleeping_orient.value = gnd_rad2deg(src->sleeping_orient.value);
gnd::conf::set_parameter(dest, &src->sleeping_orient);
// reconvert unit of angle (rad2deg)
src->sleeping_orient.value = gnd_deg2rad(src->sleeping_orient.value);
gnd::conf::set_parameter(dest, &src->cull);
gnd::conf::set_parameter(dest, &src->blur);
gnd::conf::set_parameter(dest, &src->scan_range);
gnd::conf::set_parameter(dest, &src->mfailure);
return 0;
}
/**
* @brief file out configure file
*/
inline
int proc_conf_write( const char* f, proc_configuration *src ){
gnd_assert(!f, -1, "invalid null pointer");
gnd_assert(!src, -1, "invalid null pointer");
{ // ---> operation
int ret;
gnd::conf::file_stream fs;
if( (ret = proc_conf_set(&fs, src)) < 0 ) return ret;
return fs.write(f);
} // <--- operation
}
/**
* @brief read configuration parameter file
* @param [in] f : configuration file name
* @param [out] dest : configuration parameter
*/
inline
int proc_conf_read(const char* f, proc_configuration* dest) {
gnd_assert(!f, -1, "invalid null pointer");
gnd_assert(!dest, -1, "invalid null pointer");
{ // ---> operation
int ret;
gnd::conf::file_stream fs;
// configuration file read
if( (ret = fs.read(f)) < 0 ) return ret;
return proc_conf_get(&fs, dest);
} // <--- operation
}
/**
* @brief get configuration parameter
* @param [in] src : configuration
* @param [out] dest : configuration parameter
*/
inline
int proc_conf_get( gnd::conf::configuration* src, proc_configuration* dest )
{
gnd_assert(!src, -1, "invalid null pointer");
gnd_assert(!dest, -1, "invalid null pointer");
gnd::conf::get_parameter( src, &dest->init_opsm_map );
gnd::conf::get_parameter( src, &dest->cmap );
gnd::conf::get_parameter( src, &dest->culling );
gnd::conf::get_parameter( src, &dest->cycle );
gnd::conf::get_parameter( src, &dest->failure_dist );
if( !gnd::conf::get_parameter( src, &dest->failure_orient) )
dest->failure_orient.value = gnd_deg2ang(dest->failure_orient.value);
gnd::conf::get_parameter( src, &dest->pause_time );
gnd::conf::get_parameter( src, &dest->pause_dist );
if( !gnd::conf::get_parameter( src, &dest->pause_orient ) )
dest->pause_orient.value = gnd_deg2ang(dest->pause_orient.value);
gnd::conf::get_parameter( src, &dest->map_update );
gnd::conf::get_parameter( src, &dest->map_update_time );
gnd::conf::get_parameter( src, &dest->map_update_dist );
if( !gnd::conf::get_parameter( src, &dest->map_update_orient) )
dest->converge_orient.value = gnd_deg2ang(dest->map_update_orient.value);
gnd::conf::get_parameter( src, &dest->optimizer );
gnd::conf::get_parameter( src, &dest->converge_dist );
if( !gnd::conf::get_parameter( src, &dest->converge_orient) )
dest->converge_orient.value = gnd_deg2ang(dest->converge_orient.value);
gnd::conf::get_parameter( src, &dest->ini_map_cnt );
gnd::conf::get_parameter( src, &dest->ini_match_cnt );
gnd::conf::get_parameter( src, &dest->ndt );
gnd::conf::get_parameter( src, &dest->cui_show );
gnd::conf::get_parameter( src, &dest->opsm_map );
gnd::conf::get_parameter( src, &dest->bmp );
gnd::conf::get_parameter( src, &dest->pos_gridsizex );
gnd::conf::get_parameter( src, &dest->pos_gridsizey );
gnd::conf::get_parameter( src, &dest->ang_rsl );
gnd::conf::get_parameter( src, &dest->odm_logname );
gnd::conf::get_parameter( src, &dest->ls_logname );
gnd::conf::get_parameter( src, &dest->trajectory_log );
gnd::conf::get_parameter( src, &dest->trajectory4route );
gnd::conf::get_parameter( src, &dest->laserpoint_log );
gnd::conf::get_parameter( src, &dest->output_dir );
gnd::conf::get_parameter( src, &dest->debug_odo_err_map );
return 0;
}
// configure file data analyze
inline
int proc_conf_get(gnd::conf::configuration *src, proc_configuration* dest) {
gnd_assert(!src, -1, "invalid null pointer");
gnd_assert(!dest, -1, "invalid null pointer");
gnd::conf::get_parameter(src, &dest->kfile);
gnd::conf::get_parameter(src, &dest->k_rwheel);
gnd::conf::get_parameter(src, &dest->k_rwheel_crot);
gnd::conf::get_parameter(src, &dest->k_lwheel);
gnd::conf::get_parameter(src, &dest->k_lwheel_crot);
gnd::conf::get_parameter(src, &dest->k_tread);
gnd::conf::get_parameter(src, &dest->k_gear);
gnd::conf::get_parameter(src, &dest->k_encoder);
gnd::conf::get_parameter(src, &dest->particles);
if( gnd::conf::get_parameter(src, &dest->pos_err_ini ) >= 3) {
dest->pos_err_ini.value[2] = gnd_deg2ang(dest->pos_err_ini.value[2]);
}
gnd::conf::get_parameter(src, &dest->sys_err_ini);
gnd::conf::get_parameter(src, &dest->resmp_rate_remain);
gnd::conf::get_parameter(src, &dest->resmp_rate_randerr);
if( gnd::conf::get_parameter(src, &dest->randerr_conf ) >= 3) {
dest->randerr_conf.value[2] = gnd_deg2ang(dest->randerr_conf.value[2]);
}
if( gnd::conf::get_parameter(src, &dest->randerr_offset_conf ) >= 3) {
dest->randerr_offset_conf.value[2] = gnd_deg2ang(dest->randerr_offset_conf.value[2]);
}
gnd::conf::get_parameter(src, &dest->syserr_conf );
gnd::conf::get_parameter(src, &dest->resmp_rate_syserr2);
gnd::conf::get_parameter(src, &dest->syserr2_conf );
gnd::conf::get_parameter(src, &dest->resmp_rate_resetsyserr);
gnd::conf::get_parameter(src, &dest->reset_syserr_conf );
gnd::conf::get_parameter(src, &dest->random_sampling);
gnd::conf::get_parameter(src, &dest->pws_id);
gnd::conf::get_parameter(src, &dest->ssm_id);
gnd::conf::get_parameter(src, &dest->gyro);
gnd::conf::get_parameter(src, &dest->gyro_vol);
gnd::conf::get_parameter(src, &dest->gyro_bits);
gnd::conf::get_parameter(src, &dest->gyro_bias);
gnd::conf::get_parameter(src, &dest->gyro_sf);
proc_conf_sampling_ratio_normalize(dest);
configure_get_covariance(dest);
return 0;
}
// configure file data analyze
inline
int proc_conf_set(gnd::conf::configuration *dest, proc_configuration* src) {
gnd_assert(!src, -1, "invalid null pointer");
gnd_assert(!dest, -1, "invalid null pointer");
gnd::conf::set_parameter(dest, &src->kfile);
gnd::conf::set_parameter(dest, &src->k_rwheel);
gnd::conf::set_parameter(dest, &src->k_rwheel_crot);
gnd::conf::set_parameter(dest, &src->k_lwheel);
gnd::conf::set_parameter(dest, &src->k_lwheel_crot);
gnd::conf::set_parameter(dest, &src->k_tread);
gnd::conf::set_parameter(dest, &src->k_gear);
gnd::conf::set_parameter(dest, &src->k_encoder);
gnd::conf::set_parameter(dest, &src->particles);
src->pos_err_ini.value[2] = gnd_ang2deg(src->pos_err_ini.value[2]);
gnd::conf::set_parameter(dest, &src->pos_err_ini);
src->pos_err_ini.value[2] = gnd_deg2ang(src->pos_err_ini.value[2]);
gnd::conf::set_parameter(dest, &src->sys_err_ini);
gnd::conf::set_parameter(dest, &src->resmp_rate_remain);
gnd::conf::set_parameter(dest, &src->resmp_rate_randerr);
src->randerr_conf.value[2] = gnd_ang2deg(src->randerr_conf.value[2]);
gnd::conf::set_parameter(dest, &src->randerr_conf);
src->randerr_conf.value[2] = gnd_deg2ang(src->randerr_conf.value[2]);
src->randerr_offset_conf.value[2] = gnd_ang2deg(src->randerr_offset_conf.value[2]);
gnd::conf::set_parameter(dest, &src->randerr_offset_conf);
src->randerr_offset_conf.value[2] = gnd_deg2ang(src->randerr_offset_conf.value[2]);
gnd::conf::set_parameter(dest, &src->resmp_rate_syserr);
gnd::conf::set_parameter(dest, &src->syserr_conf);
gnd::conf::set_parameter(dest, &src->resmp_rate_syserr2);
gnd::conf::set_parameter(dest, &src->syserr2_conf);
gnd::conf::set_parameter(dest, &src->resmp_rate_resetsyserr);
gnd::conf::set_parameter(dest, &src->reset_syserr_conf);
gnd::conf::set_parameter(dest, &src->random_sampling);
gnd::conf::set_parameter(dest, &src->pws_id);
gnd::conf::set_parameter(dest, &src->ssm_id);
gnd::conf::set_parameter(dest, &src->gyro);
gnd::conf::set_parameter(dest, &src->gyro_vol);
gnd::conf::set_parameter(dest, &src->gyro_bits);
gnd::conf::set_parameter(dest, &src->gyro_bias);
gnd::conf::set_parameter(dest, &src->gyro_sf);
return 0;
}
inline
int proc_conf_sampling_ratio_normalize(proc_configuration *conf)
{
gnd_assert(!conf, -1, "invalid null pointer");
{
double sum = 0;
sum += conf->resmp_rate_remain.value;
sum += conf->resmp_rate_randerr.value;
sum += conf->resmp_rate_syserr.value;
sum += conf->resmp_rate_syserr2.value;
sum += conf->resmp_rate_resetsyserr.value;
if( sum <= 0 ){
return -1;
}
else {
conf->resmp_rate_remain.value /= sum;
conf->resmp_rate_randerr.value /= sum;
conf->resmp_rate_syserr.value /= sum;
conf->resmp_rate_syserr2.value /= sum;
conf->resmp_rate_resetsyserr.value /= sum;
}
}
return 0;
}
/**
* @brief initialize process configuration structure
* @param [in/out] conf : initialized
*/
inline
int proc_conf_initialize(proc_configuration *conf) {
gnd_assert(!conf, -1, "invalid null pointer");
::memcpy(&conf->bmp_map, &ConfIni_BMPMap, sizeof(ConfIni_BMPMap));
::memcpy(&conf->raw_map, &ConfIni_RawMap, sizeof(ConfIni_RawMap));
::memcpy(&conf->sokuikiraw_name, &ConfIni_SokuikiRawName, sizeof(ConfIni_SokuikiRawName) );
::memcpy(&conf->sokuikiraw_id, &ConfIni_SokuikiRawID, sizeof(ConfIni_SokuikiRawID) );
::memcpy(&conf->sokuikifs_name, &ConfIni_SokuikiFSName, sizeof(ConfIni_SokuikiFSName) );
::memcpy(&conf->sokuikifs_id, &ConfIni_SokuikiFSID, sizeof(ConfIni_SokuikiFSID) );
::memcpy(&conf->odometry_name, &ConfIni_OdometryName, sizeof(ConfIni_OdometryName) );
::memcpy(&conf->odometry_id, &ConfIni_OdometryID, sizeof(ConfIni_OdometryID) );
::memcpy(&conf->particle_name, &ConfIni_ParticleName, sizeof(ConfIni_ParticleName) );
::memcpy(&conf->particle_id, &ConfIni_ParticleID, sizeof(ConfIni_ParticleID) );
::memcpy(&conf->eval_name, &ConfIni_ParticleEvalName, sizeof(ConfIni_ParticleEvalName) );
::memcpy(&conf->eval_id, &ConfIni_ParticleEvalID, sizeof(ConfIni_ParticleEvalID) );
::memcpy(&conf->cycle, &ConfIni_Cycle, sizeof(ConfIni_Cycle));
::memcpy(&conf->sleeping_time, &ConfIni_SleepingTime, sizeof(ConfIni_SleepingTime));
::memcpy(&conf->sleeping_dist, &ConfIni_SleepingDist, sizeof(ConfIni_SleepingDist));
::memcpy(&conf->sleeping_orient, &ConfIni_SleepingOrient, sizeof(ConfIni_SleepingOrient));
::memcpy(&conf->cull, &ConfIni_Cull, sizeof(ConfIni_Cull));
::memcpy(&conf->blur, &ConfIni_Blur, sizeof(ConfIni_Blur));
::memcpy(&conf->scan_range, &ConfIni_ScanRangeDist, sizeof(ConfIni_ScanRangeDist));
::memcpy(&conf->matching_false, &ConfIni_MatchingFailureRate, sizeof(ConfIni_MatchingFailureRate));
::memcpy(&conf->false_positive, &ConfIni_MatchingFalsePositive, sizeof(ConfIni_MatchingFalsePositive));
return 0;
}
/*!
* @brief initialize configure
*/
inline
int proc_conf_initialize(proc_configuration *conf) {
gnd_assert(!conf, -1, "invalid null pointer");
::memcpy(&conf->kfile, &ConfIni_KFile, sizeof(ConfIni_KFile) );
::memcpy(&conf->output_ssmname, &ConfIni_SSMName, sizeof(ConfIni_SSMName) );
::memcpy(&conf->output_ssmid, &ConfIni_SSMID, sizeof(ConfIni_SSMID) );
::memcpy(&conf->odmerr_name, &ConfIni_OdometryErrSSMLogFile, sizeof(ConfIni_OdometryErrSSMLogFile) );
::memcpy(&conf->odmerr_id, &ConfIni_OdometryErrID, sizeof(ConfIni_OdometryErrID) );
::memcpy(&conf->ad_ssmname, &ConfIni_ADSSMLogFile, sizeof(ConfIni_ADSSMLogFile) );
::memcpy(&conf->ad_ssmid, &ConfIni_ADSSMID, sizeof(ConfIni_ADSSMID) );
::memcpy(&conf->ad_bits, &ConfIni_ADBits, sizeof(ConfIni_ADBits) );
::memcpy(&conf->ratio_port, &ConfIni_RatioPort, sizeof(ConfIni_RatioPort) );
::memcpy(&conf->voltage, &ConfIni_Voltage, sizeof(ConfIni_Voltage) );
::memcpy(&conf->bias, &ConfIni_Bias, sizeof(ConfIni_Bias) );
::memcpy(&conf->scale_factor, &ConfIni_ScaleFactor, sizeof(ConfIni_ScaleFactor) );
::memcpy(&conf->motor_ssmname, &ConfIni_MotorSSMName, sizeof(ConfIni_MotorSSMName) );
::memcpy(&conf->motor_ssmid, &ConfIni_MotorSSMID, sizeof(ConfIni_MotorSSMID) );
::memcpy(&conf->spur_adjust, &ConfIni_SpurAdjust, sizeof(ConfIni_SpurAdjust) );
::memcpy(&conf->gyrodometry, &ConfIni_Gyrodometry, sizeof(ConfIni_Gyrodometry) );
::memcpy(&conf->error_simulation, &ConfIni_ErrorSimulation, sizeof(ConfIni_ErrorSimulation) );
::memcpy(&conf->error_distributuion, &ConfIni_ErrorDistribution, sizeof(ConfIni_ErrorDistribution) );
::memcpy(&conf->debug, &ConfIni_Debug, sizeof(ConfIni_Debug) );
return 0;
}
inline
int queue<T>::insert(const uint64_t i, const T* src, const uint64_t n)
{
gnd_assert(!src, -1, "invalid argument");
gnd_assert(i > _n, -1, "out of buffer");
gnd_error(n == 0, 0, "ineffectual argument");
// reallocate
while(_nalloc < _n + n) __reallocate__(_n + n);
// move
if(i != _n) __move__(_data + i + n, _data + i, (_n - i));
// copy
__copy__(_data + i, src, n);
_n += n;
return (int)i;
}
/*
* @brief set configuration parameter
*/
inline
int proc_conf_set(gnd::conf::configuration *dest, proc_configuration *src)
{
gnd_assert(!dest, -1, "invalid null pointer");
gnd_assert(!src, -1, "invalid null pointer");
gnd::conf::set_parameter(dest, &src->raw_name);
gnd::conf::set_parameter(dest, &src->raw_id);
gnd::conf::set_parameter(dest, &src->fs_name);
gnd::conf::set_parameter(dest, &src->fs_id);
gnd::conf::set_parameter(dest, &src->fs_storage);
gnd::conf::set_parameter(dest, &src->gl_name);
gnd::conf::set_parameter(dest, &src->gl_id);
gnd::conf::set_parameter(dest, &src->gl_pos_name);
gnd::conf::set_parameter(dest, &src->gl_pos_id);
gnd::conf::set_parameter(dest, &src->gl_storage);
gnd::conf::set_parameter(dest, &src->gl_txtlog);
return 0;
}
/**
* @breif erase comment paragraph
* @param[out] str : string
*/
inline
int file_stream::erase_comment(char *str)
{
char *p;
gnd_assert(!str, -1, "invalid null pointer");
if( !(p = ::strchr(str, TokenComment)) ) return 0;
*p = '\0';
return 0;
}
/**
* @breif erase non-blank character from str head
* @param[in/out] str
*/
inline
int file_stream::erase_nonblank(char *str)
{
int i;
gnd_assert(!str, -1, "invalid null pointer");
for(i = 0; str[i] != '\0' && !::isspace(str[i]); i++);
::memmove(str, str + i, ::strlen(str + i) + 1);
return i;
}
inline
int queue<T>::copy(const T* src, const uint64_t n)
{
gnd_assert(!src, -1, "invalid arugment");
gnd_error(n == 0, 0, "ineffectual argument");
// reallocate
while(_nalloc < _n + n) __reallocate__(_n + n);
__copy__(_data, src, n);
_n = n;
return 0;
}
inline
int queue<T>::move(const uint64_t i, T* dest, const uint64_t n)
{
gnd_assert(i + n >= _n, -1, "out of buffer");
gnd_error(n == 0, 0, "ineffectual argument");
// copy
if(dest) __copy__(dest, _data + i, n);
// move
if(i + n != _n) __move__(_data + i, _data + i + n, (_n - i - n));
_n -= n;
return 0;
}
/*!
* @brief initialize configure
*/
inline
int proc_conf_initialize(proc_configuration *conf){
gnd_assert(!conf, -1, "invalid null pointer");
::memcpy(&conf->raw_name, &ConfIni_RawSSMName, sizeof(ConfIni_RawSSMName) );
::memcpy(&conf->raw_id, &ConfIni_RawSSMID, sizeof(ConfIni_RawSSMID) );
::memcpy(&conf->fs_name, &ConfIni_FSSSMName, sizeof(ConfIni_FSSSMName) );
::memcpy(&conf->fs_id, &ConfIni_FSSSMID, sizeof(ConfIni_FSSSMID) );
::memcpy(&conf->fs_storage, &ConfIni_FSStorageLife, sizeof(ConfIni_FSStorageLife) );
::memcpy(&conf->gl_name, &ConfIni_GLSSMName, sizeof(ConfIni_GLSSMName) );
::memcpy(&conf->gl_pos_name, &ConfIni_GLPosSSMName, sizeof(ConfIni_GLPosSSMName) );
::memcpy(&conf->gl_pos_id, &ConfIni_GLPosSSMID, sizeof(ConfIni_GLPosSSMID) );
::memcpy(&conf->gl_id, &ConfIni_GLSSMID, sizeof(ConfIni_GLSSMID) );
::memcpy(&conf->gl_storage, &ConfIni_GLStorageLife, sizeof(ConfIni_GLStorageLife) );
::memcpy(&conf->gl_txtlog, &ConfIni_GLLogFile, sizeof(ConfIni_GLLogFile) );
return 0;
}
/*!
* @brief initialize configure
*/
inline
int proc_conf_initialize(proc_configuration *conf){
gnd_assert(!conf, -1, "invalid null pointer");
::memcpy(&conf->kfile, &ConfIni_KFile, sizeof(ConfIni_KFile));
::memcpy(&conf->k_lwheel, &ConfIni_KLeftWheel, sizeof(ConfIni_KLeftWheel));
::memcpy(&conf->k_lwheel_crot, &ConfIni_KLeftWheelCRot, sizeof(ConfIni_KLeftWheelCRot));
::memcpy(&conf->k_rwheel, &ConfIni_KRightWheel, sizeof(ConfIni_KRightWheel));
::memcpy(&conf->k_rwheel_crot, &ConfIni_KRightWheelCRot, sizeof(ConfIni_KRightWheelCRot));
::memcpy(&conf->k_tread, &ConfIni_KTread, sizeof(ConfIni_KTread));
::memcpy(&conf->k_gear, &ConfIni_KGear, sizeof(ConfIni_KGear));
::memcpy(&conf->k_encoder, &ConfIni_KEncoder, sizeof(ConfIni_KEncoder));
::memcpy(&conf->particles, &ConfIni_Particles, sizeof(ConfIni_Particles));
::memcpy(&conf->resmp_rate_remain, &ConfIni_ResampleRate_Remain, sizeof(ConfIni_ResampleRate_Remain));
::memcpy(&conf->pos_err_ini, &ConfIni_Initial_PositionError, sizeof(ConfIni_Initial_PositionError));
::memcpy(&conf->sys_err_ini, &ConfIni_Initial_SysErrorParam, sizeof(ConfIni_Initial_SysErrorParam));
::memcpy(&conf->randerr_conf, &ConfIni_RandomError, sizeof(ConfIni_RandomError));
::memcpy(&conf->resmp_rate_randerr, &ConfIni_ResampleRate_RandomError, sizeof(ConfIni_ResampleRate_RandomError));
::memcpy(&conf->randerr_offset_conf, &ConfIni_RandomErrorOffset, sizeof(ConfIni_RandomErrorOffset));
::memcpy(&conf->resmp_rate_syserr, &ConfIni_ResampleRate_SysError, sizeof(ConfIni_ResampleRate_SysError));
::memcpy(&conf->syserr_conf, &ConfIni_SysError, sizeof(ConfIni_SysError));
::memcpy(&conf->resmp_rate_syserr2, &ConfIni_ResampleRate_SysError2, sizeof(ConfIni_ResampleRate_SysError2));
::memcpy(&conf->syserr2_conf, &ConfIni_SysError2, sizeof(ConfIni_SysError2));
::memcpy(&conf->resmp_rate_resetsyserr, &ConfIni_ResampleRate_ResetSysError, sizeof(ConfIni_ResampleRate_ResetSysError));
::memcpy(&conf->reset_syserr_conf, &ConfIni_ResetSysError, sizeof(ConfIni_ResetSysError));
::memcpy(&conf->random_sampling, &ConfIni_RandomSampling, sizeof(ConfIni_RandomSampling));
::memcpy(&conf->pws_id, &ConfIni_PWSSSM, sizeof(ConfIni_PWSSSM));
::memcpy(&conf->ssm_id, &ConfIni_SSMID, sizeof(ConfIni_SSMID));
::memcpy(&conf->gyro, &ConfIni_Gyro, sizeof(ConfIni_Gyro));
::memcpy(&conf->gyro_vol, &ConfIni_GyroVoltage, sizeof(ConfIni_GyroVoltage));
::memcpy(&conf->gyro_bits, &ConfIni_GyroADBits, sizeof(ConfIni_GyroADBits));
::memcpy(&conf->gyro_bias, &ConfIni_GyroBias, sizeof(ConfIni_GyroBias));
::memcpy(&conf->gyro_sf, &ConfIni_GyroScaleFactor, sizeof(ConfIni_GyroScaleFactor));
proc_conf_sampling_ratio_normalize(conf);
configure_get_covariance(conf);
return 0;
}
inline
int configure_get_covariance(proc_configuration *conf)
{
gnd_assert(!conf, -1, "invalid null pointer");
gnd::matrix::set_unit( &conf->poserr_cover_ini );
for(size_t i = 0; i < 3; i++)
gnd::matrix::set(&conf->poserr_cover_ini, i, i, gnd_square( conf->pos_err_ini.value[i] ) );
gnd::matrix::set_unit( &conf->randerr_covar );
for(size_t i = 0; i < 3; i++)
gnd::matrix::set(&conf->randerr_covar, i, i, gnd_square( conf->randerr_conf.value[i] ) );
gnd::matrix::set_unit( &conf->randerr_covar_offset );
for(size_t i = 0; i < 3; i++)
gnd::matrix::set(&conf->randerr_covar_offset, i, i, gnd_square( conf->randerr_offset_conf.value[i] ) );
return 0;
}
/**
* @brief initialize configure
*/
inline
int proc_conf_initialize(proc_configuration *conf){
gnd_assert(!conf, -1, "invalid null pointer");
::memcpy(&conf->init_opsm_map, &ConfIni_ScanMatchingMapDir, sizeof(ConfIni_ScanMatchingMapDir) );
::memcpy(&conf->cmap, &ConfIni_CorrectionMapPath, sizeof(ConfIni_CorrectionMapPath) );
::memcpy(&conf->odm_logname, &ConfIni_OdometryLogName, sizeof(ConfIni_OdometryLogName) );
::memcpy(&conf->ls_logname, &ConfIni_LaserScannerLogName, sizeof(ConfIni_LaserScannerLogName) );
::memcpy(&conf->culling, &ConfIni_Culling, sizeof(ConfIni_Culling) );
::memcpy(&conf->cycle, &ConfIni_Cycle, sizeof(ConfIni_Cycle) );
::memcpy(&conf->failure_dist, &ConfIni_FailDist, sizeof(ConfIni_FailDist) );
::memcpy(&conf->failure_orient, &ConfIni_FailOrient, sizeof(ConfIni_FailOrient) );
::memcpy(&conf->pause_time, &ConfIni_RestCycle, sizeof(ConfIni_RestCycle) );
::memcpy(&conf->pause_dist, &ConfIni_RestDist, sizeof(ConfIni_RestDist) );
::memcpy(&conf->pause_orient, &ConfIni_RestOrient, sizeof(ConfIni_RestOrient) );
::memcpy(&conf->map_update, &ConfIni_SLAM, sizeof(ConfIni_SLAM) );
::memcpy(&conf->map_update_time, &ConfIni_MapUpdateTime, sizeof(ConfIni_MapUpdateTime) );
::memcpy(&conf->map_update_dist, &ConfIni_MapUpdateDist, sizeof(ConfIni_MapUpdateDist) );
::memcpy(&conf->map_update_orient, &ConfIni_MapUpdateOrient, sizeof(ConfIni_MapUpdateOrient) );
::memcpy(&conf->optimizer, &ConfIni_Optimizer, sizeof(ConfIni_Optimizer) );
::memcpy(&conf->converge_dist, &ConfIni_ConvergeDist, sizeof(ConfIni_ConvergeDist) );
::memcpy(&conf->converge_orient, &ConfIni_ConvergeOrient, sizeof(ConfIni_ConvergeOrient) );
::memcpy(&conf->ini_map_cnt, &ConfIni_InitMapCnt, sizeof(ConfIni_InitMapCnt) );
::memcpy(&conf->ini_match_cnt, &ConfIni_InitMatchingCnt, sizeof(ConfIni_InitMatchingCnt) );
::memcpy(&conf->ndt, &ConfIni_NDT, sizeof(ConfIni_NDT) );
::memcpy(&conf->cui_show, &ConfIni_DebugShowMode, sizeof(ConfIni_DebugShowMode) );
::memcpy(&conf->opsm_map, &ConfIni_OPSMMap, sizeof(ConfIni_OPSMMap) );
::memcpy(&conf->bmp, &ConfIni_BMP, sizeof(ConfIni_BMP) );
::memcpy(&conf->pos_gridsizex, &ConfIni_PosGridSizeX, sizeof(ConfIni_PosGridSizeX) );
::memcpy(&conf->pos_gridsizey, &ConfIni_PosGridSizeY, sizeof(ConfIni_PosGridSizeY) );
::memcpy(&conf->ang_rsl, &ConfIni_AngReslution, sizeof(ConfIni_AngReslution) );
::memcpy(&conf->trajectory_log, &ConfIni_TrajectoryLog, sizeof(ConfIni_TrajectoryLog) );
::memcpy(&conf->trajectory4route, &ConfIni_Trajectory4Route, sizeof(ConfIni_Trajectory4Route) );
::memcpy(&conf->laserpoint_log, &ConfIni_LaserPointLog, sizeof(ConfIni_LaserPointLog) );
::memcpy(&conf->output_dir, &ConfIni_OutputDir, sizeof(ConfIni_OutputDir) );
::memcpy(&conf->debug_odo_err_map, &ConfIni_DebugOdometryErrorMap, sizeof(ConfIni_DebugOdometryErrorMap) );
return 0;
}
/**
* @brief file out configure file
*/
inline
int proc_conf_set( gnd::conf::configuration* dest, proc_configuration* src ) {
gnd_assert(!dest, -1, "invalid null pointer");
gnd_assert(!src, -1, "invalid null pointer");
{ // ---> operation
gnd::conf::set_parameter(dest, &src->odm_logname);
gnd::conf::set_parameter(dest, &src->ls_logname);
// scan matching parameter
gnd::conf::set_parameter(dest, &src->cycle);
gnd::conf::set_parameter(dest, &src->culling);
gnd::conf::set_parameter(dest, &src->optimizer);
gnd::conf::set_parameter(dest, &src->converge_dist);
src->converge_orient.value = gnd_ang2deg(src->converge_orient.value);
gnd::conf::set_parameter(dest, &src->converge_orient);
src->converge_orient.value = gnd_deg2ang(src->converge_orient.value);
gnd::conf::set_parameter(dest, &src->failure_dist);
src->failure_orient.value = gnd_ang2deg(src->failure_orient.value);
gnd::conf::set_parameter(dest, &src->failure_orient);
src->failure_orient.value = gnd_deg2ang(src->failure_orient.value);
gnd::conf::set_parameter(dest, &src->pause_time);
gnd::conf::set_parameter(dest, &src->pause_dist);
src->pause_orient.value = gnd_ang2deg(src->pause_orient.value);
gnd::conf::set_parameter(dest, &src->pause_orient );
src->pause_orient.value = gnd_deg2ang(src->pause_orient.value);
gnd::conf::set_parameter(dest, &src->ini_map_cnt );
gnd::conf::set_parameter(dest, &src->ini_match_cnt );
gnd::conf::set_parameter(dest, &src->ndt);
gnd::conf::set_parameter(dest, &src->map_update);
gnd::conf::set_parameter(dest, &src->map_update_time);
gnd::conf::set_parameter(dest, &src->map_update_dist);
src->map_update_orient.value = gnd_ang2deg(src->map_update_orient.value);
gnd::conf::set_parameter(dest, &src->map_update_orient);
src->map_update_orient.value = gnd_deg2ang(src->map_update_orient.value);
gnd::conf::set_parameter(dest, &src->output_dir );
gnd::conf::set_parameter(dest, &src->opsm_map );
gnd::conf::set_parameter(dest, &src->bmp );
gnd::conf::set_parameter(dest, &src->pos_gridsizex );
gnd::conf::set_parameter(dest, &src->pos_gridsizey );
gnd::conf::set_parameter(dest, &src->ang_rsl );
gnd::conf::set_parameter(dest, &src->trajectory_log );
gnd::conf::set_parameter(dest, &src->trajectory4route );
gnd::conf::set_parameter(dest, &src->laserpoint_log );
gnd::conf::set_parameter(dest, &src->cui_show );
gnd::conf::set_parameter(dest, &src->init_opsm_map);
gnd::conf::set_parameter(dest, &src->cmap );
return 0;
} // <--- operation
}
inline bool options::get_option(int aArgc, char **aArgv)
{
gnd_assert(!param, -1, "parameter storage is null.");
while(1){
int opt;
optarg = 0;
opt = ::getopt_long(aArgc, aArgv, ShortOpt, LongOpt, 0);
if(opt < 0) break;
switch(opt){
// read configure
case 'g':
{
gnd::conf::file_stream conf_fs;
if( conf_fs.read(optarg) < 0 ){
::fprintf(stderr, " ... [\x1b[1m\x1b[31mERROR\x1b[30m\x1b[0m]: -g option, Fail to read configure file\n");
return RFail;
}
if( proc_conf_get(&conf_fs, param) < 0){
::fprintf(stderr, " ... [\x1b[1m\x1b[31mERROR\x1b[30m\x1b[0m]: -g option, configure file syntax error\n");
return RFail;
}
} break;
// write configure
case 'G': {
proc_conf_write( optarg ? optarg : "ls-coordinate-converter.conf", param);
::fprintf(stdout, " ... output configuration file \"\x1b[4m%s\x1b[0m\"\n", optarg ? optarg : "ls-coordinate-converter.conf");
} return RWriteConf;
// write configure
// show help
case 'h':
{
int i = 0;
fprintf(stderr, "\t\x1b[1mNAME\x1b[0m\n");
fprintf(stderr, "\t\t\x1b[1m%s\x1b[0m - laser scan coordinate convert \n", proc_name);
fprintf(stderr, "\n");
fprintf(stderr, "\t\x1b[1mSYNAPSIS\x1b[0m\n");
fprintf(stderr, "\t\t\x1b[1m%s\x1b[0m [\x1b[4mOPTIONS\x1b[0m]\n", proc_name);
fprintf(stderr, "\n");
fprintf(stderr, "\t\x1b[1mDISCRIPTION\x1b[0m\n");
fprintf(stderr, "\t\t\x1b[1m%s\x1b[0m convert the coordinate of laser scan.\n", proc_name);
fprintf(stderr, "\n");
fprintf(stderr, "\t\x1b[1mOPTIONS\x1b[0m\n");
fprintf(stderr, "\t\t\x1b[1m-%c\x1b[0m, \x1b[1m--%s\x1b[0m\n", LongOpt[i].val, LongOpt[i].name);
fprintf(stderr, "\t\t\tprint help\n");
fprintf(stderr, "\n");
i++;
fprintf(stderr, "\t\t\x1b[1m-%c\x1b[0m, \x1b[1m--%s\x1b[0m\n", LongOpt[i].val, LongOpt[i].name);
fprintf(stderr, "\t\t\tread configure file\n");
fprintf(stderr, "\n");
i++;
fprintf(stderr, "\t\t\x1b[1m-%c\x1b[0m, \x1b[1m--%s\x1b[0m\n", LongOpt[i].val, LongOpt[i].name);
fprintf(stderr, "\t\t\twirte configure file\n");
fprintf(stderr, "\n");
i++;
return RHelp;
}break;
}
}
return 0;
}
