RobotConfiguration* readLog(std::vector<BaseSensorData*>& sensorDatas, Deserializer& des){
RobotConfiguration* conf = 0;
Serializable *o;
int numObjects=0;
while( (! conf && (o=des.readObject())) ){
cerr << o->className() << endl;
numObjects++;
if (! conf) {
conf = dynamic_cast<RobotConfiguration*>(o);
if (conf) {
cerr << "got config" << endl;
if (! conf->isReady()){
cerr << "conf failure" << endl;
return 0;
}
}
continue;
}
}
if (!conf) {
cerr << "unable to read robot configuration, aborting" << endl;
return 0;
}
while( (o=des.readObject()) ){
BaseSensorData* sensorData=dynamic_cast<BaseSensorData*>(o);
if(sensorData)
sensorDatas.push_back(sensorData);
}
return conf;
}
int main(int argc, char** argv) {
std::list<CommandArg> parsedArgs;
if (argc == 1) {
printBanner();
return 0;
}
int c = parseArgs(parsedArgs, argc, argv);
if (c<argc-2) {
printBanner();
return 0;
}
// create a synchronizer
Synchronizer sync;
handleParsedArgs(&sync, parsedArgs);
std::string filein = argv[c];
std::string fileout = argv[c+1];
Deserializer des;
des.setFilePath(filein.c_str());
sync.addSyncTopic("/camera/depth_registered/image_rect_raw");
Serializer ser;
ser.setFilePath(fileout.c_str());
cerr << "running logger with arguments: filein[" << filein << "] fileout: [" << fileout << "]" << endl;
std::vector<BaseSensorData*> sensorDatas;
RobotConfiguration* conf = readLog(sensorDatas, des);
cerr << "# frames: " << conf->frameMap().size() << endl;
cerr << "# sensors: " << conf->sensorMap().size() << endl;
cerr << "# sensorDatas: " << sensorDatas.size() << endl;
conf->serializeInternals(ser);
ser.writeObject(*conf);
TSCompare comp;
std::sort(sensorDatas.begin(), sensorDatas.end(), comp);
MapManager* manager = new MapManager();
ser.writeObject(*manager);
SensingFrameNodeMaker* nodeMaker = new SensingFrameNodeMaker();
nodeMaker->init(manager,conf);
StreamProcessor::PropagatorOutputHandler* sync2nm=new StreamProcessor::PropagatorOutputHandler(&sync, nodeMaker);
OdometryRelationAdder* odometryAdder = new OdometryRelationAdder(manager,conf);
StreamProcessor::PropagatorOutputHandler* nm2odom=new StreamProcessor::PropagatorOutputHandler(nodeMaker, odometryAdder);
ImuRelationAdder* imuAdder = new ImuRelationAdder(manager,conf);
StreamProcessor::PropagatorOutputHandler* odom2imu=new StreamProcessor::PropagatorOutputHandler(odometryAdder, imuAdder);
StreamProcessor::WriterOutputHandler* writer = new StreamProcessor::WriterOutputHandler(imuAdder, &ser);
//OdometryRelationAdder* odometryAdder = new OdometryRelationAdder(manager, conf);
MARKUSED(sync2nm);
MARKUSED(nm2odom);
MARKUSED(odom2imu);
MARKUSED(writer);
for (size_t i = 0; i< sensorDatas.size(); i++){
BaseSensorData* data = sensorDatas[i];
sync.process(data);
}
}
int main(int argc, char** argv) {
/************************************************************************
* Input Handling *
************************************************************************/
string configFile;
string logFile;
CommandArgs arg;
int nscale;
int mscale;
// Optional input parameters.
arg.param("nscale", nscale, 1, "image scaling for the normal extraction");
arg.param("mscale", mscale, 1, "image scaling for matching");
// Last parameter has to be the working directory.
arg.paramLeftOver("config_file", configFile, "", "file where the configuration will be written", true);
arg.paramLeftOver("log_file", logFile, "", "synchronized log file", true);
arg.parseArgs(argc, argv);
//Aligner* aligner;
//DepthImageConverter* converter;
cerr << "processing log file [" << logFile << "] with parameters read from [" << configFile << "]" << endl;
cerr << "reading the configuration from file [" << configFile << "]" << endl;
std::vector<Serializable*> alignerInstances = readConfig(argv[0], aligner, converter, configFile);
cerr<< "Aligner: " << aligner << endl;
cerr<< "Converter: " << converter << endl;
if (logFile=="") {
cerr << "logfile not supplied" << endl;
}
Deserializer des;
des.setFilePath(logFile);
std::vector<BaseSensorData*> sensorDatas;
RobotConfiguration* conf = readLog(sensorDatas, des);
cerr << "# frames: " << conf->frameMap().size() << endl;
cerr << "# sensors: " << conf->sensorMap().size() << endl;
cerr << "# sensorDatas: " << sensorDatas.size() << endl;
TSCompare comp;
std::sort(sensorDatas.begin(), sensorDatas.end(), comp);
MapManager* manager = new MapManager;
SensingFrameNodeMaker* sensingFrameMaker = new SensingFrameNodeMaker;
sensingFrameMaker->init(manager, conf);
ImuRelationAdder* imuAdder = new ImuRelationAdder(manager, conf);
OdometryRelationAdder* odomAdder = new OdometryRelationAdder(manager, conf);
TwoDepthImageAlignerNode* pairwiseAligner = new TwoDepthImageAlignerNode(manager, conf, converter, aligner, "/kinect/depth_registered/image_raw");
for (size_t i = 0; i<sensorDatas.size(); i++) {
// see if you make a sensing frame with all the infos you find
SensingFrameNode* s = sensingFrameMaker->processData(sensorDatas[i]);
if (s) {
// add a unary relation modeling the imu to the sensing frame
imuAdder->processNode(s);
// add a binary relation modeling the odometry between two sensing frames
odomAdder->processNode(s);
// add a pwn sensing data(if you manage)
pairwiseAligner->processNode(s);
}
}
cerr << "writing out things" << endl;
Serializer ser;
ser.setFilePath("out.log");
for (size_t i = 0; i< alignerInstances.size(); i++) {
ser.writeObject(*alignerInstances[i]);
}
conf->serializeInternals(ser);
ser.writeObject(*conf);
ReferenceFrame* lastFrame = 0;
for (size_t i = 0; i<sensorDatas.size(); i++) {
if (lastFrame != sensorDatas[i]->robotReferenceFrame()) {
lastFrame = sensorDatas[i]->robotReferenceFrame();
ser.writeObject(*lastFrame);
}
ser.writeObject(*sensorDatas[i]);
}
ser.writeObject(*manager);
for (std::set<MapNode*>::iterator it = manager->nodes().begin(); it!=manager->nodes().end(); it++)
ser.writeObject(**it);
cerr << "writing out " << manager->relations().size() << " relations" << endl;
for (std::set<MapNodeRelation*>::iterator it = manager->relations().begin(); it!=manager->relations().end(); it++)
ser.writeObject(**it);
// write the aligner stuff
// write the robot configuration
// write the sensor data
// write the manager
// write the objects in the manager
// // write back the log
// Serializer ser;
// ser.setFilePath("sensing_frame_test.log");
// conf->serializeInternals(ser);
// ser.writeObject(*conf);
// previousReferenceFrame = 0;
// for (size_t i = 0; i< sensorDatas.size(); i++){
// BaseSensorData* data = sensorDatas[i];
// if (previousReferenceFrame!=data->robotReferenceFrame()){
// ser.writeObject(*data->robotReferenceFrame());
// }
// ser.writeObject(*data);
// previousReferenceFrame = data->robotReferenceFrame();
// }
// for(size_t i=0; i<newObjects.size(); i++){
// ser.writeObject(*newObjects[i]);
// }
}
int main(int argc, char** argv) {
std::list<CommandArg> parsedArgs;
if (argc == 1) {
printBanner();
return 0;
}
int c = parseArgs(parsedArgs, argc, argv);
if (c<argc-2) {
printBanner();
return 0;
}
// create a synchronizer
Synchronizer sync;
handleParsedArgs(&sync, parsedArgs);
std::string filein = argv[c];
std::string fileout = argv[c+1];
Deserializer des;
des.setFilePath(filein.c_str());
Serializer ser;
ser.setFilePath(fileout.c_str());
cerr << "running logger with arguments: filein[" << filein << "] fileout: [" << fileout << "]" << endl;
// create a reframer object, that once all messages have been put together sets them to a unique frame
// Synchronizer::Reframer reframer;
// sync.addOutputHandler(&reframer);
// // create a writer object that dumps on the disk each block of synchronized objects
// Synchronizer::Writer writer(&ser);
// sync.addOutputHandler(&writer);
// // create a deleter object that polishes the memory after writing
// Synchronizer::Deleter deleter;
// sync.addOutputHandler(&deleter);
StreamProcessor::WriterOutputHandler* writer = new StreamProcessor::WriterOutputHandler(&sync, &ser);
std::vector<BaseSensorData*> sensorDatas;
RobotConfiguration* conf = readLog(sensorDatas, des);
cerr << "# frames: " << conf->frameMap().size() << endl;
cerr << "# sensors: " << conf->sensorMap().size() << endl;
cerr << "# sensorDatas: " << sensorDatas.size() << endl;
conf->serializeInternals(ser);
ser.writeObject(*conf);
TSCompare comp;
std::sort(sensorDatas.begin(), sensorDatas.end(), comp);
for (size_t i = 0; i< sensorDatas.size(); i++){
BaseSensorData* data = sensorDatas[i];
sync.process(data);
}
}
