void monteCarlo()
{
float sonar = SensorValue(sonar_sensor) - 6; // allow for distance between sonar and centre of wheelbase
//nxtDisplayCenteredTextLine(1, "sonar: %f", (float)sonar);
if (sonar < 10 || sonar > 150) {return;}
measurementUpdate(sonar);
normalisation();
resampling();
}
void SelfLocator::update(PotentialRobotPose& robotPose)
{
//MODIFY("module:SelfLocator:parameter", *parameter);
// recreate sampleset if number of samples was changed
ASSERT(samples);
if(parameter->numberOfSamples != samples->size())
init();
// Maybe pose has already been computed by call to other update function (for clusters)
if(lastPoseComputationTimeStamp == bb->theFrameInfo.time)
{
robotPose = (PotentialRobotPose&)lastComputedPose;
return;
}
// Normal computation:
preExecution(robotPose);
bool templatesOnly(false);
bool odometryOnly(false);
//DEBUG_RESPONSE("module:SelfLocator:templates only", templatesOnly = true;);
//DEBUG_RESPONSE("module:SelfLocator:odometry only", odometryOnly = true;);
if(templatesOnly) //debug
{
if(sampleTemplateGenerator.templatesAvailable())
for(int i = 0; i < this->samples->size(); ++i)
generateTemplate(samples->at(i));
poseCalculator->calcPose(robotPose);
}
else if(odometryOnly) //debug
{
robotPose += bb->theOdometryData - lastOdometry;
lastOdometry = bb->theOdometryData;
}
else //normal case
{
motionUpdate(updatedBySensors);
updatedBySensors = applySensorModels();
if(updatedBySensors)
{
adaptWeightings();
resampling();
}
poseCalculator->calcPose(robotPose);
}
robotPose.deviation = 100000.f;
lastComputedPose = robotPose;
lastPoseComputationTimeStamp = bb->theFrameInfo.time;
// drawings
/*DECLARE_DEBUG_DRAWING("module:SelfLocator:poseCalculator", "drawingOnField"); // Draws the internal representations for pose computation
COMPLEX_DRAWING("module:SelfLocator:poseCalculator", poseCalculator->draw());
DECLARE_DEBUG_DRAWING("module:SelfLocator:perceptValidityChecker", "drawingOnField"); // Draws the internal representations for percept validity checking
COMPLEX_DRAWING("module:SelfLocator:perceptValidityChecker", perceptValidityChecker->draw());
DECLARE_DEBUG_DRAWING("module:SelfLocator:samples", "drawingOnField"); // Draws the internal representations of the goal locator
COMPLEX_DRAWING("module:SelfLocator:samples", drawSamples());
DECLARE_DEBUG_DRAWING("module:SelfLocator:templates", "drawingOnField"); // Draws all available templates
COMPLEX_DRAWING("module:SelfLocator:templates", sampleTemplateGenerator.draw(););
DECLARE_DEBUG_DRAWING("module:SelfLocator:selectedPoints", "drawingOnField");*/
//fieldModel.draw();
}
//-----------------------------------------------------------------------------
bool AudioFile::Load()
{
#if MAC
AudioFileID mAudioFileID;
AudioStreamBasicDescription fileDescription, outputFormat;
SInt64 dataSize64;
UInt32 dataSize;
OSStatus err;
UInt32 size;
// ファイルを開く
FSRef ref;
Boolean isDirectory=false;
FSPathMakeRef((const UInt8*)GetFilePath(), &ref, &isDirectory);
err = AudioFileOpen(&ref, fsRdPerm, 0, &mAudioFileID);
if (err) {
//NSLog(@"AudioFileOpen failed");
return false;
}
// 開いたファイルの基本情報を fileDescription へ
size = sizeof(AudioStreamBasicDescription);
err = AudioFileGetProperty(mAudioFileID, kAudioFilePropertyDataFormat,
&size, &fileDescription);
if (err) {
//NSLog(@"AudioFileGetProperty failed");
AudioFileClose(mAudioFileID);
return false;
}
// 開いたファイルのデータ部のバイト数を dataSize へ
size = sizeof(SInt64);
err = AudioFileGetProperty(mAudioFileID, kAudioFilePropertyAudioDataByteCount,
&size, &dataSize64);
if (err) {
//NSLog(@"AudioFileGetProperty failed");
AudioFileClose(mAudioFileID);
return false;
}
dataSize = static_cast<UInt32>(dataSize64);
AudioFileTypeID fileTypeID;
size = sizeof( AudioFileTypeID );
err = AudioFileGetProperty(mAudioFileID, kAudioFilePropertyFileFormat, &size, &fileTypeID);
if (err) {
//NSLog(@"AudioFileGetProperty failed");
AudioFileClose(mAudioFileID);
return false;
}
// Instrument情報を初期化
mInstData.basekey = 60;
mInstData.lowkey = 0;
mInstData.highkey = 127;
mInstData.loop = 0;
//ループポイントの取得
Float64 st_point=0.0,end_point=0.0;
if ( fileTypeID == kAudioFileAIFFType || fileTypeID == kAudioFileAIFCType ) {
//INSTチャンクの取得
AudioFileGetUserDataSize(mAudioFileID, 'INST', 0, &size);
if ( size > 4 ) {
UInt8 *instChunk = new UInt8[size];
AudioFileGetUserData(mAudioFileID, 'INST', 0, &size, instChunk);
//MIDI情報の取得
mInstData.basekey = instChunk[0];
mInstData.lowkey = instChunk[2];
mInstData.highkey = instChunk[3];
if ( instChunk[9] > 0 ) { //ループフラグを確認
//マーカーの取得
UInt32 writable;
err = AudioFileGetPropertyInfo(mAudioFileID, kAudioFilePropertyMarkerList,
&size, &writable);
if (err) {
//NSLog(@"AudioFileGetPropertyInfo failed");
AudioFileClose(mAudioFileID);
return NULL;
}
UInt8 *markersBuffer = new UInt8[size];
AudioFileMarkerList *markers = reinterpret_cast<AudioFileMarkerList*>(markersBuffer);
err = AudioFileGetProperty(mAudioFileID, kAudioFilePropertyMarkerList,
&size, markers);
if (err) {
//NSLog(@"AudioFileGetProperty failed");
AudioFileClose(mAudioFileID);
return NULL;
}
//ループポイントの設定
for (unsigned int i=0; i<markers->mNumberMarkers; i++) {
if (markers->mMarkers[i].mMarkerID == instChunk[11] ) {
st_point = markers->mMarkers[i].mFramePosition;
}
else if (markers->mMarkers[i].mMarkerID == instChunk[13] ) {
end_point = markers->mMarkers[i].mFramePosition;
}
CFRelease(markers->mMarkers[i].mName);
}
if ( st_point < end_point ) {
mInstData.loop = 1;
}
delete [] markersBuffer;
}
delete [] instChunk;
}
}
else if ( fileTypeID == kAudioFileWAVEType ) {
//smplチャンクの取得
AudioFileGetUserDataSize( mAudioFileID, 'smpl', 0, &size );
if ( size >= sizeof(WAV_smpl) ) {
UInt8 *smplChunk = new UInt8[size];
AudioFileGetUserData( mAudioFileID, 'smpl', 0, &size, smplChunk );
WAV_smpl *smpl = (WAV_smpl *)smplChunk;
smpl->loops = EndianU32_LtoN( smpl->loops );
if ( smpl->loops > 0 ) {
mInstData.loop = true;
mInstData.basekey = EndianU32_LtoN( smpl->note );
st_point = EndianU32_LtoN( smpl->start );
end_point = EndianU32_LtoN( smpl->end ) + 1; //SoundForge等では最終ポイントを含める解釈
//end_point = EndianU32_LtoN( smpl->end ); //PeakではなぜかAIFFと同じ
}
else {
mInstData.basekey = EndianU32_LtoN( smpl->note );
}
delete [] smplChunk;
}
}
//容量の制限
SInt64 dataSamples = dataSize / fileDescription.mBytesPerFrame;
if ( dataSamples > MAXIMUM_SAMPLES ) {
dataSize = MAXIMUM_SAMPLES * fileDescription.mBytesPerFrame;
}
if ( st_point > MAXIMUM_SAMPLES ) {
st_point = MAXIMUM_SAMPLES;
}
if ( end_point > MAXIMUM_SAMPLES ) {
end_point = MAXIMUM_SAMPLES;
}
// 波形一時読み込み用メモリを確保
char *fileBuffer;
unsigned int fileBufferSize;
if (mInstData.loop) {
fileBufferSize = dataSize+EXPAND_BUFFER*fileDescription.mBytesPerFrame;
}
else {
fileBufferSize = dataSize;
}
fileBuffer = new char[fileBufferSize];
memset(fileBuffer, 0, fileBufferSize);
// ファイルから波形データの読み込み
err = AudioFileReadBytes(mAudioFileID, false, 0, &dataSize, fileBuffer);
if (err) {
//NSLog(@"AudioFileReadBytes failed");
AudioFileClose(mAudioFileID);
delete [] fileBuffer;
return false;
}
AudioFileClose(mAudioFileID);
//ループを展開する
Float64 adjustment = 1.0;
outputFormat=fileDescription;
if (mInstData.loop) {
UInt32 plusalpha=0, framestocopy;
while (plusalpha < EXPAND_BUFFER) {
framestocopy =
(end_point-st_point)>(EXPAND_BUFFER-plusalpha)?(EXPAND_BUFFER-plusalpha):end_point-st_point;
memcpy(fileBuffer+((int)end_point+plusalpha)*fileDescription.mBytesPerFrame,
fileBuffer+(int)st_point*fileDescription.mBytesPerFrame,
framestocopy*fileDescription.mBytesPerFrame);
plusalpha += framestocopy;
}
dataSize += plusalpha*fileDescription.mBytesPerFrame;
//16サンプル境界にFIXする
adjustment = ( (long long)((end_point-st_point)/16) ) / ((end_point-st_point)/16.0);
st_point *= adjustment;
end_point *= adjustment;
}
outputFormat.mFormatID = kAudioFormatLinearPCM;
outputFormat.mFormatFlags = kAudioFormatFlagIsFloat | kAudioFormatFlagsNativeEndian;
outputFormat.mChannelsPerFrame = 1;
outputFormat.mBytesPerFrame = sizeof(float);
outputFormat.mBitsPerChannel = 32;
outputFormat.mBytesPerPacket = outputFormat.mBytesPerFrame;
// バイトオーダー変換用のコンバータを用意
AudioConverterRef converter;
err = AudioConverterNew(&fileDescription, &outputFormat, &converter);
if (err) {
//NSLog(@"AudioConverterNew failed");
delete [] fileBuffer;
return false;
}
//サンプリングレート変換の質を最高に設定
// if (fileDescription.mSampleRate != outputFormat.mSampleRate) {
// size = sizeof(UInt32);
// UInt32 setProp = kAudioConverterQuality_Max;
// AudioConverterSetProperty(converter, kAudioConverterSampleRateConverterQuality,
// size, &setProp);
//
// size = sizeof(UInt32);
// setProp = kAudioConverterSampleRateConverterComplexity_Mastering;
// AudioConverterSetProperty(converter, kAudioConverterSampleRateConverterComplexity,
// size, &setProp);
//
// }
//出力に必要十分なバッファサイズを得る
UInt32 outputSize = dataSize;
size = sizeof(UInt32);
err = AudioConverterGetProperty(converter, kAudioConverterPropertyCalculateOutputBufferSize,
&size, &outputSize);
if (err) {
//NSLog(@"AudioConverterGetProperty failed");
delete [] fileBuffer;
AudioConverterDispose(converter);
return false;
}
UInt32 monoSamples = outputSize/sizeof(float);
// バイトオーダー変換
float *monoData = new float[monoSamples];
AudioConverterConvertBuffer(converter, dataSize, fileBuffer,
&outputSize, monoData);
if(outputSize == 0) {
//NSLog(@"AudioConverterConvertBuffer failed");
delete [] fileBuffer;
AudioConverterDispose(converter);
return false;
}
//ループ長が16の倍数でない場合はサンプリングレート変換
Float64 inputSampleRate = fileDescription.mSampleRate;
Float64 outputSampleRate = fileDescription.mSampleRate * adjustment;
int outSamples = monoSamples;
if ( outputSampleRate == inputSampleRate ) {
m_pAudioData = new short[monoSamples];
for (int i=0; i<monoSamples; i++) {
m_pAudioData[i] = static_cast<short>(monoData[i] * 32768);
}
}
else {
outSamples = static_cast<int>(monoSamples / (inputSampleRate / outputSampleRate));
m_pAudioData = new short[outSamples];
resampling(monoData, monoSamples, inputSampleRate,
m_pAudioData, &outSamples, outputSampleRate);
}
// 後始末
delete [] monoData;
delete [] fileBuffer;
AudioConverterDispose(converter);
//Instデータの設定
if ( st_point > MAXIMUM_SAMPLES ) {
mInstData.lp = MAXIMUM_SAMPLES;
}
else {
mInstData.lp = st_point;
}
if ( end_point > MAXIMUM_SAMPLES ) {
mInstData.lp_end = MAXIMUM_SAMPLES;
}
else {
mInstData.lp_end = end_point;
}
mInstData.srcSamplerate = outputSampleRate;
mLoadedSamples = outSamples;
mIsLoaded = true;
return true;
#else
//Windowsのオーディオファイル読み込み処理
// ファイルを開く
HMMIO hmio = NULL;
MMRESULT err;
DWORD size;
hmio = mmioOpen( mPath, NULL, MMIO_READ );
if ( !hmio ) {
return false;
}
// RIFFチャンクを探す
MMCKINFO riffChunkInfo;
riffChunkInfo.fccType = mmioFOURCC('W', 'A', 'V', 'E');
err = mmioDescend( hmio, &riffChunkInfo, NULL, MMIO_FINDRIFF );
if ( err != MMSYSERR_NOERROR ) {
mmioClose( hmio, 0 );
return false;
}
if ( (riffChunkInfo.ckid != FOURCC_RIFF) || (riffChunkInfo.fccType != mmioFOURCC('W', 'A', 'V', 'E') ) ) {
mmioClose( hmio, 0 );
return false;
}
// フォーマットチャンクを探す
MMCKINFO formatChunkInfo;
formatChunkInfo.ckid = mmioFOURCC('f', 'm', 't', ' ');
err = mmioDescend( hmio, &formatChunkInfo, &riffChunkInfo, MMIO_FINDCHUNK );
if ( err != MMSYSERR_NOERROR ) {
mmioClose( hmio, 0 );
return false;
}
if ( formatChunkInfo.cksize < sizeof(PCMWAVEFORMAT) ) {
mmioClose( hmio, 0 );
return false;
}
//フォーマット情報を取得
WAVEFORMATEX pcmWaveFormat;
DWORD fmsize = (formatChunkInfo.cksize > sizeof(WAVEFORMATEX)) ? sizeof(WAVEFORMATEX):formatChunkInfo.cksize;
size = mmioRead( hmio, (HPSTR)&pcmWaveFormat, fmsize );
if ( size != fmsize ) {
mmioClose( hmio, 0 );
return false;
}
if ( pcmWaveFormat.wFormatTag != WAVE_FORMAT_PCM ) {
mmioClose( hmio, 0 );
return false;
}
mmioAscend(hmio, &formatChunkInfo, 0);
// Instrument情報を初期化
mInstData.basekey = 60;
mInstData.lowkey = 0;
mInstData.highkey = 127;
mInstData.loop = 0;
//smplチャンクを探す
MMCKINFO smplChunkInfo;
smplChunkInfo.ckid = mmioFOURCC('s', 'm', 'p', 'l');
err = mmioDescend( hmio, &smplChunkInfo, &riffChunkInfo, MMIO_FINDCHUNK );
if ( err != MMSYSERR_NOERROR ) {
smplChunkInfo.cksize = 0;
}
double st_point=0.0;
double end_point=0.0;
if ( smplChunkInfo.cksize >= sizeof(WAV_smpl) ) {
//ループポイントの取得
unsigned char *smplChunk = new unsigned char[smplChunkInfo.cksize];
size = mmioRead(hmio,(HPSTR)smplChunk, smplChunkInfo.cksize);
WAV_smpl *smpl = (WAV_smpl *)smplChunk;
if ( smpl->loops > 0 ) {
mInstData.loop = 1;
mInstData.basekey = smpl->note;
st_point = smpl->start;
end_point = smpl->end + 1; //SoundForge等では最終ポイントを含める解釈
}
else {
mInstData.basekey = smpl->note;
}
delete [] smplChunk;
}
mmioAscend(hmio, &formatChunkInfo, 0);
//dataチャンクを探す
MMCKINFO dataChunkInfo;
dataChunkInfo.ckid = mmioFOURCC('d', 'a', 't', 'a');
err = mmioDescend( hmio, &dataChunkInfo, &riffChunkInfo, MMIO_FINDCHUNK );
if( err != MMSYSERR_NOERROR ) {
mmioClose( hmio, 0 );
return false;
}
// 波形一時読み込み用メモリを確保
unsigned int dataSize = dataChunkInfo.cksize;
int bytesPerSample = pcmWaveFormat.nBlockAlign;
char *fileBuffer;
unsigned int fileBufferSize;
//容量制限
int dataSamples = dataSize / pcmWaveFormat.nBlockAlign;
if ( dataSamples > MAXIMUM_SAMPLES ) {
dataSize = MAXIMUM_SAMPLES * pcmWaveFormat.nBlockAlign;
}
if ( st_point > MAXIMUM_SAMPLES ) {
st_point = MAXIMUM_SAMPLES;
}
if ( end_point > MAXIMUM_SAMPLES ) {
end_point = MAXIMUM_SAMPLES;
}
if (mInstData.loop) {
fileBufferSize = dataSize+EXPAND_BUFFER*bytesPerSample;
}
else {
fileBufferSize = dataSize;
}
fileBuffer = new char[fileBufferSize];
memset(fileBuffer, 0, fileBufferSize);
// ファイルから波形データの読み込み
size = mmioRead(hmio, (HPSTR)fileBuffer, dataSize);
if ( size != dataSize ) {
mmioClose( hmio, 0 );
return false;
}
mmioClose(hmio,0);
//ループを展開する
double inputSampleRate = pcmWaveFormat.nSamplesPerSec;
double outputSampleRate = inputSampleRate;
if (mInstData.loop) {
unsigned int plusalpha=0;
double framestocopy;
while (plusalpha < EXPAND_BUFFER) {
framestocopy =
(end_point-st_point)>(EXPAND_BUFFER-plusalpha)?(EXPAND_BUFFER-plusalpha):end_point-st_point;
memcpy(fileBuffer+((int)end_point+plusalpha)*bytesPerSample,
fileBuffer+(int)st_point*bytesPerSample,
static_cast<size_t>(framestocopy*bytesPerSample));
plusalpha += static_cast<unsigned int>(framestocopy);
}
dataSize += plusalpha*bytesPerSample;
//16サンプル境界にFIXする
double adjustment = ( (long long)((end_point-st_point)/16) ) / ((end_point-st_point)/16.0);
outputSampleRate *= adjustment;
st_point *= adjustment;
end_point *= adjustment;
}
//一旦floatモノラルデータに変換
int bytesPerChannel = bytesPerSample / pcmWaveFormat.nChannels;
unsigned int inputPtr = 0;
unsigned int outputPtr = 0;
int monoSamples = dataSize / bytesPerSample;
float range = static_cast<float>((1<<(bytesPerChannel*8-1)) * pcmWaveFormat.nChannels);
float *monoData = new float[monoSamples];
while (inputPtr < dataSize) {
int frameSum = 0;
for (int ch=0; ch<pcmWaveFormat.nChannels; ch++) {
for (int i=0; i<bytesPerChannel; i++) {
if (i<bytesPerChannel-1) {
frameSum += (unsigned char)fileBuffer[inputPtr] << (8*i);
}
else {
frameSum += fileBuffer[inputPtr] << (8*i);
}
inputPtr++;
}
}
monoData[outputPtr] = frameSum / range;
outputPtr++;
}
//ループ長が16の倍数でない場合はサンプリングレート変換
int outSamples = monoSamples;
if ( outputSampleRate == inputSampleRate ) {
m_pAudioData = new short[monoSamples];
for (int i=0; i<monoSamples; i++) {
m_pAudioData[i] = static_cast<short>(monoData[i] * 32768);
}
}
else {
outSamples = static_cast<int>(monoSamples / (inputSampleRate / outputSampleRate));
m_pAudioData = new short[outSamples];
resampling(monoData, monoSamples, inputSampleRate,
m_pAudioData, &outSamples, outputSampleRate);
}
// 後始末
delete [] fileBuffer;
delete [] monoData;
//Instデータの設定
mInstData.lp = static_cast<int>(st_point);
mInstData.lp_end = static_cast<int>(end_point);
mInstData.srcSamplerate = outputSampleRate;
mLoadedSamples = outSamples;
mIsLoaded = true;
return true;
#endif
}
void SemanticBagOfWordsTrainer::train()
{
//get samples file
std::string training_samples_file = TO_INFO(std::string,getInputPort(INPUT_PORT_SAMPLES_INFO));
Matrix<float> samples_label,samples_representation;
{
Matrix<float> training_samples;
EagleeyeIO::read(training_samples,m_trainer_folder + training_samples_file + m_ext_name,READ_BINARY_MODE);
int samples_num = training_samples.rows();
samples_label = training_samples(Range(0,samples_num),Range(0,1));
samples_label.clone();
samples_representation = training_samples(Range(0,samples_num),Range(1,training_samples.cols()));
samples_representation.clone();
}
EagleeyeIO semantic_models_o;
semantic_models_o.createWriteHandle(m_trainer_folder + m_trainer_name + m_ext_name,false,WRITE_BINARY_MODE);
//write class number
semantic_models_o.write(m_class_num);
for (int class_index = 0; class_index < 2; ++class_index)
{
EAGLEEYE_INFO("process model %d\n",class_index);
Matrix<float> resampling_label = samples_label;
resampling_label.clone();
int samples_num = resampling_label.rows();
//reassign positive and negative samples
for (int i = 0; i < samples_num; ++i)
{
if (int(resampling_label(i)) == class_index)
resampling_label(i) = 0.0f;
else
resampling_label(i) = 1.0f;
}
Matrix<float> resampling_samples = samples_representation;
resampling_samples.clone();
resampling(resampling_samples,resampling_label);
int resamples_num = resampling_label.rows();
//reconstruct sample feature
Matrix<float> words_frequency = resampling_samples(Range(0,resamples_num),Range(0,m_words_num));
Matrix<float> words_pair_dis = resampling_samples(Range(0,resamples_num),Range(m_words_num,m_words_num + m_clique_num));
Matrix<float> words_pair_angle = resampling_samples(Range(0,resamples_num),Range(m_words_num + m_clique_num,m_words_num + 2 * m_clique_num));
Matrix<int> target_states(resamples_num,m_words_num);
for (int sample_index = 0; sample_index < resamples_num; ++sample_index)
{
if (int(resampling_label(sample_index)) == 0)
{
//positive samples
for (int w_index = 0; w_index < m_words_num; ++w_index)
{
if (words_frequency(sample_index,w_index) > 0.0001f)
target_states(sample_index,w_index) = 1;
else
target_states(sample_index,w_index) = 0;
}
}
else
{
//negative samples
for (int w_index = 0; w_index < m_words_num; ++w_index)
{
if (words_frequency(sample_index,w_index) > 0.0001f)
target_states(sample_index,w_index) = 2;
else
target_states(sample_index,w_index) = 0;
}
}
}
/* putToMatlab(target_states,"target");*/
//start training
SemanticBagOfWords* semantic_model = new SemanticBagOfWords(m_words_num,3);
semantic_model->semanticLearn(target_states,words_frequency,words_pair_dis,words_pair_angle);
Matrix<float> unary_coe,clique_coe;
semantic_model->getModelParam(unary_coe,clique_coe);
semantic_models_o.write(unary_coe);
semantic_models_o.write(clique_coe);
delete semantic_model;
}
semantic_models_o.destroyHandle();
//write classifier model info
InfoSignal<std::string>* output_signal_calssifier_info =
TO_INFO_SIGNAL(std::string,getOutputPort(OUTPUT_PORT_CLASSIFIER_INFO));
m_trainer_info = m_trainer_name;
output_signal_calssifier_info->info = &m_trainer_info;
//evaluate performance
}
