void pa5functions::BinSort( RecordList& recordList, RecordList* bucketList ){
for( RecordList::iterator it = recordList.begin();
it != recordList.end();
++ it ){
bucketList[it->testScore].push_back( (*it) );
}
}
//----[ load ]-------------------------------------------------------------
bool dcResourceStorage::load(const std::string& owner_name,
dcGenericResource* resource) {
typedef std::queue<dcGenericResource*> LoadingQueue;
LoadingQueue loading_queue;
loading_queue.push(resource);
std::string root_path = owner_name + dc::dcGenericResource::PATH_SEPARATOR;
do {
dcGenericResource* resource = loading_queue.front();
// Load members of the current resource
RecordList members;
if (!read(root_path + resource->getPathString(), &members)) return false;
RecordList::iterator i;
for (i = members.begin(); i != members.end(); ++i) {
dcGenericResource* member =
resource->acquireMember(
i->member_name.c_str(),
i->resource_type.c_str(),
0);
if (member) {
member->interpret(i->value.c_str());
loading_queue.push(member);
}
}
// We're done loading this resource, so remove it from the queue
loading_queue.pop();
} while (!loading_queue.empty());
return true;
}
void computeBoundingBox (double& xmin, double& ymin, double& xmax, double& ymax, const RecordList& rl, double maxrange)
{
xmin = ymin = MAXDOUBLE;
xmax = ymax = -MAXDOUBLE;
const LaserRecord* lastLaser = 0;
for (RecordList::const_iterator it = rl.begin(); it != rl.end(); it++)
{
const LaserRecord* lr = dynamic_cast<const LaserRecord*> (*it);
if (lr)
{
lastLaser = lr;
continue;
}
const ScanMatchRecord* smr = dynamic_cast<const ScanMatchRecord*> (*it);
if (smr && lastLaser)
{
for (std::vector<OrientedPoint>::const_iterator pit = smr->poses.begin(); pit != smr->poses.end(); pit++)
{
computeBoundingBox (xmin, ymin, xmax, ymax, *lastLaser, *pit, maxrange);
}
}
}
}
void
CmdInterface::show(const RecordList & lst)
{
cIter i = lst.begin();
for ( ; i != lst.end(); i++)
CmdInterface::show(*i);
}
void
CmdInterface::show(const RecordList & lst)
{
if (lst.size() > 0)
{
cIter i = lst.begin();
for ( ; i != lst.end(); i++)
CmdInterface::show(*i);
}
else
println("empty.");
}
int main(int argc, char ** argv) {
if (argc<2) {
cout << "usage gfs2stat <infilename> <outfilename>" << endl;
return 0;
}
ifstream is(argv[1]);
if (!is) {
cout << "no file found: " << argv[1] << endl;
return 0;
}
ofstream os(argv[2]);
if (!os) {
cout << "cannot open file: " << argv[1] << endl;
return 0;
}
cout << "loading... "<< flush;
RecordList rl;
rl.read(is);
cout << " done" << endl;
int count=-1;
for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++) {
count++;
const ScanMatchRecord* rec=dynamic_cast<const ScanMatchRecord*>(*it);
if (!rec)
continue;
Gaussian3 gaussian;
/*
vector<double> nweights;
cout << "N"<< flush;
back_insert_iterator< vector<double> > out(nweights);
toNormalForm(out,rec->weights.begin(), rec->weights.end());
cout << "G"<< flush;
gaussian.computeFromSamples(rec->poses, nweights);
*/
gaussian.computeFromSamples(rec->poses);
cout << "E"<< flush;
os << count <<" ";
os << gaussian.mean.x <<" ";
os << gaussian.mean.y <<" ";
os << gaussian.mean.theta <<" ";
os << gaussian.covariance.eval[0] <<" ";
os << gaussian.covariance.eval[1] <<" ";
os << gaussian.covariance.eval[2] <<endl;
}
os.close();
}
//-------------------------------------------------------------------------------------------------
void BachAssetListView::dropEvent( QDropEvent * event )
{
if ( !mShowingCollection || !bach_editor_mode )
return;
DEBG( "dropEvent 1!" );
if ( event->source() == this &&
mCurrentDropTarget.isValid() &&
mCurrentCollection.isValid() )
{
DEBG( "dropEvent 2.2!" );
if ( !RecordDrag::canDecode( event->mimeData() ) )
return;
BachAssetList bal;
GetAssets( mCurrentCollection, mShowExcluded, bal );
RecordList dropped;
RecordDrag::decode( event->mimeData(), &dropped );
BachAssetIter it = bal.begin();
// first, do all the items UP to the item into which it should be inserted before
int position = 0;
for( ; it != bal.end() ; ++it )
{
BachAsset ba = (*it);
if ( mCurrentDropTarget == ba )
break;
if ( dropped.contains( ba ) )
continue;
BachBucketMap bbm = BachBucketMap::recordByBucketAndAsset( mCurrentCollection, ba );
DEBG( "Position1:"+QString::number(position)+":"+ba.path() );
bbm.setPosition( position );
bbm.commit();
++position;
}
// then do all the dropped ones
for ( RecordIter it = dropped.begin() ; it != dropped.end() ; ++it )
{
BachAsset ba = *it;
// find the map used
BachBucketMap bbm = BachBucketMap::recordByBucketAndAsset( mCurrentCollection, ba );
DEBG( "Position2:"+QString::number(position)+":"+bbm.bachAsset().path() );
bbm.setPosition( position );
bbm.commit();
++position;
}
// then do all the ones past the end
for( ; it != bal.end() ; ++it )
{
BachAsset ba = (*it);
if ( dropped.contains( ba ) )
continue;
BachBucketMap bbm = BachBucketMap::recordByBucketAndAsset( mCurrentCollection, ba );
DEBG( "Position3:"+QString::number(position)+":"+ba.path() );
bbm.setPosition( position );
bbm.commit();
++position;
}
BachAssetList bal1;
GetAssets( mCurrentCollection, mShowExcluded, bal1 );
model()->updateRecords( BachAssetList() );
model()->append( bal1 );
event->acceptProposedAction();
}
}
//---------------------------------------------------------------------------
void BinexReadWriteTest::process()
throw()
{
if (verboseLevel > 0)
{
cout << "Creating BINEX records . . ." << endl;
}
for (short recNum = 0; recNum < 10; recNum++)
{
BinexData record(recNum);
TestDataList recordData;
size_t offset = 0;
for (short dataNum = 0; dataNum < 80; dataNum++)
{
TestDataType whichType = (TestDataType)(rand() % (eMGFZI + 1) );
void *value = NULL;
switch (whichType)
{
case eChar:
{
char c = (char)(rand() % 0x100);
record.updateMessageData(offset, c, sizeof(c) );
value = new char(c);
break;
}
case eShort:
{
short s = (short)(rand() % 10000);
record.updateMessageData(offset, s, sizeof(s) );
value = new short(s);
break;
}
case eLong:
{
long l = (long)(lrand48() );
record.updateMessageData(offset, l, sizeof(l) );
value = new long(l);
break;
}
case eUBNXI:
{
BinexData::UBNXI u( (unsigned long)(abs(lrand48() ) ) % BinexData::UBNXI::MAX_VALUE);
record.updateMessageData(offset, u);
value = new BinexData::UBNXI(u);
break;
}
case eMGFZI:
{
BinexData::MGFZI m( (long long)(lrand48() ) );
record.updateMessageData(offset, m);
value = new BinexData::MGFZI(m);
break;
}
default:
// Internal error
exit(1);
}
if (value != NULL)
{
recordData.push_back(TestData(whichType, value) );
}
}
testData.push_back(recordData);
testRecords.push_back(record);
}
if (verboseLevel > 0)
{
cout << "Verifying BINEX records . . ." << endl;
}
TestDataListList::iterator dataListIter = testData.begin();
RecordList::iterator recordIter = testRecords.begin();
bool more = true;
while ( (dataListIter != testData.end() )
&& (recordIter != testRecords.end() ) )
{
TestDataList dataList = *dataListIter;
BinexData record = *recordIter;
try
{
size_t offset = 0;
TestDataList::iterator dataIter = (*dataListIter).begin();
while (dataIter != (*dataListIter).end() )
{
switch ( (*dataIter).first)
{
case eChar:
{
string desc = "Comparing character record message data";
char c;
record.extractMessageData(offset, c, sizeof(c) );
if (memcmp( (void*)&c, (*dataIter).second, sizeof(c) ) )
{
report(desc, false);
cout << " Actual: " << c << endl;
cout << " Expected: " << *( (char*)(*dataIter).second) << endl;
}
else
{
report(desc, true);
}
break;
}
case eShort:
{
string desc = "Comparing short record message data";
short s;
record.extractMessageData(offset, s, sizeof(s) );
if (memcmp( (void*)&s, (*dataIter).second, sizeof(s) ) )
{
report(desc, false);
cout << " Actual: " << s << endl;
cout << " Expected: " << *( (char*)(*dataIter).second) << endl;
}
else
{
report(desc, true);
}
break;
}
case eLong:
{
string desc = "Comparing long record message data";
long l;
record.extractMessageData(offset, l, sizeof(l) );
if (memcmp( (void*)&l, (*dataIter).second, sizeof(l) ) )
{
report(desc, false);
cout << " Actual: " << l << endl;
cout << " Expected: " << *( (char*)(*dataIter).second) << endl;
}
else
{
report(desc, true);
}
break;
}
case eUBNXI:
{
string desc = "Comparing UBNXI record message data";
BinexData::UBNXI u;
record.extractMessageData(offset, u);
if (u == *( (BinexData::UBNXI*)(*dataIter).second) )
{
report(desc, true);
}
else
{
report(desc, false);
cout << " Actual: " << (unsigned long)u << endl;
cout << " Expected: " << (unsigned long)*( (BinexData::UBNXI*)(*dataIter).second) << endl;
}
break;
}
case eMGFZI:
{
string desc = "Comparing MGFZI record message data";
BinexData::MGFZI m;
record.extractMessageData(offset, m);
if (m == *( (BinexData::MGFZI*)(*dataIter).second) )
{
report(desc, true);
}
else
{
report(desc, false);
cout << " Actual: " << (long long)m << endl;
cout << " Expected: " << (long long)*( (BinexData::MGFZI*)(*dataIter).second) << endl;
}
break;
}
default:
// Internal error
exit(1);
}
dataIter++;
}
}
catch (FFStreamError e)
{
cout << " FFStreamError reading record." << endl;
}
catch (...)
{
cout << " Unknown error reading record." << endl;
}
dataListIter++;
recordIter++;
}
if (verboseLevel > 0)
{
cout << "Writing BINEX file . . ." << endl;
}
BinexStream outStream("test.out", std::ios::out | std::ios::binary);
outStream.exceptions(ios_base::failbit | ios_base::badbit);
recordIter = testRecords.begin();
while (recordIter != testRecords.end() )
{
try
{
(*recordIter).putRecord(outStream);
}
catch(...)
{
cout << " Error writing record." << endl;
}
recordIter++;
}
outStream.close();
if (verboseLevel > 0)
{
cout << "Reading BINEX file . . ." << endl;
}
BinexStream inStream("test.out", std::ios::in | std::ios::binary);
inStream.exceptions(ios_base::failbit);
recordIter = testRecords.begin();
while (inStream.good() && (EOF != inStream.peek() ) )
{
if (recordIter == testRecords.end() )
{
cout << "Stored records exhausted before file records - exiting." << endl;
break;
}
BinexData record;
try
{
record.getRecord(inStream);
if (record == *recordIter)
{
report("Reading and comparing BINEX record", true);
}
else
{
report("Reading and comparing BINEX record", false);
cout << "Actual record:" << endl;
(*recordIter).dump(cout);
cout << "Expected record:" << endl;
record.dump(cout);
}
}
catch (FFStreamError e)
{
cout << e << endl;
}
catch (...)
{
cout << " Unknown error reading record." << endl;
}
recordIter++;
}
inStream.close();
}
int main (int argc, char** argv)
{
QApplication app (argc, argv);
double maxrange = 50;
double delta = 0.1;
int scanSkip = 5;
const char* filename = 0;
const char* format = "PNG";
CMD_PARSE_BEGIN (1, argc)
parseDouble ("-maxrange", maxrange);
parseDouble ("-delta", delta);
parseInt ("-skip", scanSkip);
parseString ("-filename", filename);
parseString ("-format", format);
CMD_PARSE_END
double maxUrange = maxrange;
if (! filename)
{
cout << " supply a gfs file, please" << endl;
cout << " usage gfs2img [options] -filename <gfs_file>" << endl;
cout << " [options]:" << endl;
cout << " -maxrange <range>" << endl;
cout << " -delta <map cell size>" << endl;
cout << " -skip <frames to skip among images>" << endl;
cout << " -format <image format in capital letters>" << endl;
return -1;
}
ifstream is (filename);
if (!is)
{
cout << " supply an EXISTING gfs file, please" << endl;
return -1;
}
RecordList rl;
rl.read (is);
int particles = 0;
int beams = 0;
for (RecordList::const_iterator it = rl.begin(); it != rl.end(); it++)
{
const OdometryRecord* odometry = dynamic_cast<const OdometryRecord*> (*it);
if (odometry)
{
particles = odometry->dim;
}
const LaserRecord* s = dynamic_cast<const LaserRecord*> (*it);
if (s)
{
beams = s->readings.size();
}
if (particles && beams)
break;
}
cout << "Particles from gfs=" << particles << endl;
if (! particles)
{
cout << "no particles found, terminating" << endl;
return -1;
}
cout << "Laser beams from gfs=" << beams << endl;
if (! beams)
{
cout << "0 beams found, terminating" << endl;
return -1;
}
double laserBeamStep = 0;
if (beams == 180 || beams == 181)
{
laserBeamStep = M_PI / 180;
}
else if (beams == 360 || beams == 361)
{
laserBeamStep = M_PI / 360;
}
cout << "Laser beam step" << laserBeamStep << endl;
if (laserBeamStep == 0)
{
cout << "Invalid Beam Step, terminating" << endl;
return -1;
}
double laserAngles[MAX_LASER_BEAMS];
double theta = -M_PI / 2;
for (int i = 0; i < beams; i++)
{
laserAngles[i] = theta;
theta += laserBeamStep;
}
ScanMatcher matcher;
matcher.setLaserParameters (beams, laserAngles, OrientedPoint (0, 0, 0));
matcher.setlaserMaxRange (maxrange);
matcher.setusableRange (maxUrange);
matcher.setgenerateMap (true);
double xmin, ymin, xmax, ymax;
cout << "computing bounding box" << endl;
computeBoundingBox (xmin, ymin, xmax, ymax, rl, maxrange);
cout << "DONE" << endl << "BBOX= " << xmin << " " << ymin << " " << xmax << " " << ymax << endl;
Point center;
center.x = (xmin + xmax) / 2.;
center.y = (ymin + ymax) / 2.;
cout << "computing paths" << endl;
unsigned int frame = 0;
int scanCount = 0;
for (RecordList::const_iterator it = rl.begin(); it != rl.end(); it++)
{
const ScanMatchRecord* s = dynamic_cast<const ScanMatchRecord*> (*it);
if (!s)
continue;
scanCount++;
if (scanCount % scanSkip)
continue;
cout << "Frame " << frame << " ";
std::vector<RecordList> paths (particles);
int bestIdx = 0;
double bestWeight = -MAXDOUBLE;
for (int p = 0; p < particles; p++)
{
paths[p] = rl.computePath (p, it);
double w = rl.getLogWeight (p, it);
if (w > bestWeight)
{
bestWeight = w;
bestIdx = p;
}
}
cout << "bestIdx=" << bestIdx << " bestWeight=" << bestWeight << endl;
cout << "computing best map" << endl;
ScanMatcherMap smap (center, xmin, ymin, xmax, ymax, delta);
int count = 0;
for (RecordList::const_iterator mt = paths[bestIdx].begin(); mt != paths[bestIdx].end(); mt++)
{
const LaserRecord* s = dynamic_cast<const LaserRecord*> (*mt);
if (s)
{
double rawreadings[MAX_LASER_BEAMS];
for (uint i = 0; i < s->readings.size(); i++)
rawreadings[i] = s->readings[i];
matcher.invalidateActiveArea();
matcher.computeActiveArea (smap, s->pose, rawreadings);
// matcher.allocActiveArea(smap, s->pose, rawreadings);
matcher.registerScan (smap, s->pose, rawreadings);
count++;
}
}
cout << "DONE " << count << endl;
QPixmap pixmap (smap.getMapSizeX(), smap.getMapSizeY());
pixmap.fill (QColor (200, 200, 255));
QPainter painter (&pixmap);
for (int x = 0; x < smap.getMapSizeX(); x++)
for (int y = 0; y < smap.getMapSizeY(); y++)
{
double v = smap.cell (x, y);
if (v >= 0)
{
int grayValue = 255 - (int) (255.*v);
painter.setPen (QColor (grayValue, grayValue, grayValue));
painter.drawPoint (x, smap.getMapSizeY() - y - 1);
}
}
/*
cout << "painting trajectories" << endl;
for (int p=0; p<particles; p++){
painter.setPen(QColor(Qt::red));
if (p==bestIdx)
continue;
bool first=true;
IntPoint oldPoint(0,0);
for (RecordList::const_iterator mt=paths[p].begin(); mt!=paths[p].end(); mt++){
const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
if (s){
IntPoint ip=smap.world2map(s->pose);
ip.y=smap.getMapSizeY()-ip.y-1;
if (!first){
painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y);
}
oldPoint=ip;
first=false;
}
}
paths[p].destroyReferences();;
}
painter.setPen(QColor(Qt::black));
bool first=true;
IntPoint oldPoint(0,0);
for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){
const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
if (s){
IntPoint ip=smap.world2map(s->pose);
ip.y=smap.getMapSizeY()-ip.y-1;
if (!first){
painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y);
}
oldPoint=ip;
first=false;
}
}
paths[bestIdx].destroyReferences();;
*/
cout << " DONE" << endl;
cout << "writing image" << endl;
QImage img = pixmap.convertToImage();
char ofilename[MAX_FILENAME];
sprintf (ofilename, "%s-%.4d.%s", filename, frame, format);
cout << ofilename << endl;
img.save (QString (ofilename), format, 0);
frame++;
}
cout << "For Cyrill: \"The Evil is Outside\"" << endl;
}