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]); // } }