int container_main(void* arg)
{
// no need to delete it because we execv finally.
CommandArgs* args = reinterpret_cast<CommandArgs*>(arg);
printf("Container [%d] - inside the container!\n", getpid());
printf("Container: eUID = %ld; eGID = %ld, UID=%ld, GID=%ld\n",
(long) geteuid(), (long) getegid(), (long) getuid(), (long) getgid());
char ch;
close(pipefd[1]);
int ret = read(pipefd[0], &ch, 1);
if (ret < 0) {
printf("Error: read failed: %d, %s\n", errno, strerror(errno));
return 1;
}
close(pipefd[0]);
printf("Container: eUID = %ld; eGID = %ld, UID=%ld, GID=%ld\n",
(long) geteuid(), (long) getegid(), (long) getuid(), (long) getgid());
mount("proc", "/proc", "proc", 0, NULL);
char** argv = args->GetExecArgs();
execv(argv[0], argv);
printf("Something's wrong!\n");
return 1;
}
void RGBLed::OnCommandReceived(CommandArgs *args)
{
if(args->Get_Type() == Get && (args->Get_Command() == All | args->Get_Command() == RGBLedCommand))
{
CommandArgs sendArgs;
sendArgs.Set_Type(Set);
sendArgs.Set_Command(RGBLedCommand);
byte values[3];
values[0] = Get_Red();
values[1] = Get_Green();
values[2] = Get_Blue();
sendArgs.Set_Length(3);
sendArgs.Set_Value(values);
SendCommand(&sendArgs);
}
else if(args->Get_Type() == Set && args->Get_Command() == RGBLedCommand)
{
byte* values = args->Get_Value();
if(values != null)
{
if(args->Get_Length() == 3)
{
Set_Red(values[0]);
Set_Green(values[1]);
Set_Blue(values[2]);
}
}
}
}
void ProxyScheduler::error(SchedulerDriver* driver, const std::string& message)
{
CommandArgs args;
args.push_back(CommandArg(message));
dispatcher_->send( MesosCommand("error", args) );
}
int main(int argc, char** argv)
{
QApplication qapp(argc, argv);
#ifdef G2O_HAVE_GLUT
glutInit(&argc, argv);
#endif
string dummy;
string inputFilename;
CommandArgs arg;
arg.param("solverlib", dummy, "", "specify a solver library which will be loaded");
arg.param("typeslib", dummy, "", "specify a types library which will be loaded");
arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);
arg.parseArgs(argc, argv);
// loading the standard solver / types
DlWrapper dlTypesWrapper;
loadStandardTypes(dlTypesWrapper, argc, argv);
// register all the solvers
DlWrapper dlSolverWrapper;
loadStandardSolver(dlSolverWrapper, argc, argv);
MainWindow mw;
mw.updateDisplayedSolvers();
mw.show();
// redirect the output that normally goes to cerr to the textedit in the viewer
StreamRedirect redirect(cerr, mw.plainTextEdit);
// setting up the optimizer
SparseOptimizer* optimizer = new SparseOptimizer();
mw.viewer->graph = optimizer;
// set up the GUI action
GuiHyperGraphAction guiHyperGraphAction;
guiHyperGraphAction.viewer = mw.viewer;
optimizer->addPostIterationAction(&guiHyperGraphAction);
if (inputFilename.size() > 0) {
mw.loadFromFile(QString::fromStdString(inputFilename));
}
while (mw.isVisible()) {
guiHyperGraphAction.dumpScreenshots = mw.actionDump_Images->isChecked();
qapp.processEvents();
SleepThread::msleep(10);
}
delete optimizer;
// destroy all the singletons
//Factory::destroy();
//OptimizationAlgorithmFactory::destroy();
//HyperGraphActionLibrary::destroy();
return 0;
}
static CommandArgs messageToCommandArgs(const ofxOscMessage& message) {
CommandArgs args;
for (int i = 0; i < message.getNumArgs(); ++i) {
auto arg = messageArgToCommandArg(message, i);
args.add(arg);
}
return args;
}
void ProxyScheduler::frameworkMessage(SchedulerDriver* driver,
const ExecutorID& executorId,
const SlaveID& slaveId,
const std::string& data)
{
CommandArgs args;
PUSH_MSG(args, executorId, "ExecutorID");
PUSH_MSG(args, slaveId, "SlaveID");
args.push_back(CommandArg(data));
dispatcher_->send( MesosCommand("frameworkMessage", args) );
}
void ProxyScheduler::executorLost(SchedulerDriver* driver,
const ExecutorID& executorId,
const SlaveID& slaveId,
int status)
{
CommandArgs args;
PUSH_MSG(args, executorId, "ExecutorID");
PUSH_MSG(args, slaveId, "SlaveID");
args.push_back(CommandArg(std::to_string(status)));
dispatcher_->send( MesosCommand("executorLost", args) );
}
void ProxyScheduler::resourceOffers(SchedulerDriver* driver,
const std::vector<Offer>& offers)
{
CommandArgs args;
std::vector<std::string> strings;
for (std::vector<Offer>::const_iterator it = offers.begin(); it != offers.end(); ++it) {
strings.push_back(it->SerializeAsString());
}
args.push_back( CommandArg(strings, "Offer") );
dispatcher_->send( MesosCommand("resourceOffers", args) );
}
int main(int argc, char** argv) {
CommandArgs arg;
std::string outputFilename;
std::string inputFilename;
arg.param("o", outputFilename, "", "output file name");
arg.paramLeftOver("input-filename ", inputFilename, "", "graph file to read", true);
arg.parseArgs(argc, argv);
OptimizableGraph graph;
if (!graph.load(inputFilename.c_str())){
cerr << "Error: cannot load a file from \"" << inputFilename << "\", aborting." << endl;
return 0;
}
HyperGraph::EdgeSet removedEdges;
HyperGraph::VertexSet removedVertices;
for (HyperGraph::EdgeSet::iterator it = graph.edges().begin(); it!=graph.edges().end(); it++) {
HyperGraph::Edge* e = *it;
EdgeSE2PointXY* edgePointXY = dynamic_cast<EdgeSE2PointXY*>(e);
if (edgePointXY) {
VertexSE2* pose = dynamic_cast<VertexSE2*>(edgePointXY->vertex(0));
VertexPointXY* landmark = dynamic_cast<VertexPointXY*>(edgePointXY->vertex(1));
FeaturePointXYData * feature = new FeaturePointXYData();
feature->setPositionMeasurement(edgePointXY->measurement());
feature->setPositionInformation(edgePointXY->information());
pose->addUserData(feature);
removedEdges.insert(edgePointXY);
removedVertices.insert(landmark);
}
}
for (HyperGraph::EdgeSet::iterator it = removedEdges.begin(); it!=removedEdges.end(); it++){
OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(*it);
graph.removeEdge(e);
}
for (HyperGraph::VertexSet::iterator it = removedVertices.begin(); it!=removedVertices.end(); it++){
OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
graph.removeVertex(v);
}
if (outputFilename.length()){
graph.save(outputFilename.c_str());
}
}
virtual bool Run(P4Task& task, const CommandArgs& args)
{
if (!task.IsConnected()) // Cannot login without being connected
{
Conn().Log().Info() << "Cannot login when not connected" << Endl;
return false;
}
ClearStatus();
m_LoggedIn = false;
m_Password = task.GetP4Password();
m_CheckingForLoggedIn = args.size() > 1;
const string cmd = string("login") + (m_CheckingForLoggedIn ? string(" " ) + args[1] : string());
if (!task.CommandRun(cmd, this) && !m_CheckingForLoggedIn)
{
string errorMessage = GetStatusMessage();
Conn().Log().Notice() << "ERROR: " << errorMessage << Endl;
}
if (m_CheckingForLoggedIn)
Conn().Log().Debug() << "Is logged in: " << (m_LoggedIn ? "yes" : "no") << Endl;
else
Conn().Log().Info() << "Login " << (m_LoggedIn ? "succeeded" : "failed") << Endl;
m_CheckingForLoggedIn = false;
return m_LoggedIn;
}
bool P4StatusCommand::Run(P4Task& task, const CommandArgs& args)
{
// Since the status command is use to check for online state we start out by
// forcing online state to true and check if it has been set to false in the
// end to determine if we should send online notifications.
bool wasOnline = P4Task::IsOnline();
P4Task::SetOnline(true);
ClearStatus();
bool recursive = args.size() > 1;
Pipe().Log().Info() << "StatusCommand::Run()" << unityplugin::Endl;
VersionedAssetList assetList;
Pipe() >> assetList;
RunAndSend(task, assetList, recursive);
Pipe() << GetStatus();
if (P4Task::IsOnline() && !wasOnline)
{
// If set to online already we cannot notify as online so we fake an offline state.
P4Task::SetOnline(false);
P4Task::NotifyOnline();
}
Pipe().EndResponse();
return true;
}
const StringWriterPtr RandomService::commandRandom(const CommandArgs& nCommandArgs)
{
StringWriterPtr stringWriter_(new StringWriter());
nCommandArgs.runStringWriter(stringWriter_);
stringWriter_->startClass("result");
const string& strMin_ = nCommandArgs.getCommandArg(1);
const string& strMax_ = nCommandArgs.getCommandArg(2);
const __i32 minValue_ = __convert<string, __i32>(strMin_);
const __i32 maxValue_ = __convert<string, __i32>(strMax_);
stringWriter_->runString(strMin_, "strMin");
stringWriter_->runInt32(minValue_, "minValue");
stringWriter_->runString(strMax_, "strMax");
stringWriter_->runInt32(maxValue_, "maxValue");
stringWriter_->finishClass();
stringWriter_->runClose();
return stringWriter_;
}
int main(int argc, char** argv)
{
OptimizableGraph::initMultiThreading();
QApplication qapp(argc, argv);
CommandArgs arg;
#ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES
string dummy;
arg.param("solverlib", dummy, "", "specify a solver library which will be loaded");
arg.param("typeslib", dummy, "", "specify a types library which will be loaded");
// loading the standard solver / types
DlWrapper dlTypesWrapper;
loadStandardTypes(dlTypesWrapper, argc, argv);
// register all the solvers
DlWrapper dlSolverWrapper;
loadStandardSolver(dlSolverWrapper, argc, argv);
#endif
// run the viewer
return RunG2OViewer::run(argc, argv, arg);
}
int main(int argc, char** argv)
{
bool pcg;
int updateEachN;
bool vis;
bool verbose;
// command line parsing
CommandArgs arg;
arg.param("update", updateEachN, 10, "update the graph after inserting N nodes");
arg.param("pcg", pcg, false, "use PCG instead of Cholesky");
arg.param("v", verbose, false, "verbose output of the optimization process");
arg.param("g", vis, false, "gnuplot visualization");
arg.parseArgs(argc, argv);
SparseOptimizerOnline optimizer(pcg);
//SparseOptimizer optimizer;
optimizer.setVerbose(verbose);
optimizer.setForceStopFlag(&hasToStop);
optimizer.vizWithGnuplot = vis;
G2oSlamInterface slamInterface(&optimizer);
slamInterface.setUpdateGraphEachN(updateEachN);
SlamParser::ParserInterface parserInterface(&slamInterface);
while (parserInterface.parseCommand(cin))
{
// do something additional if needed
}
return 0;
}
bool P4EditCommand::Run(P4Task &task)
{
CommandArgs myArgs;
// Append changelist if available
if(!m_changelist.empty())
{
myArgs.emplace_back("-c");
myArgs.emplace_back(m_changelist);
}
// Append custom arguments
std::copy(m_customArgs.begin(), m_customArgs.end(), std::back_inserter(myArgs));
// Append paths
for(auto &path : m_paths)
{
myArgs.emplace_back(path);
}
return task.runP4Command("edit", myArgs, this);
}
int RunG2OViewer::run(int argc, char** argv, CommandArgs& arg)
{
std::string inputFilename;
arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);
arg.parseArgs(argc, argv);
MainWindow mw;
mw.updateDisplayedSolvers();
mw.updateRobustKernels();
mw.show();
// redirect the output that normally goes to cerr to the textedit in the viewer
StreamRedirect redirect(std::cerr, mw.plainTextEdit);
// setting up the optimizer
SparseOptimizer* optimizer = new SparseOptimizer();
mw.viewer->graph = optimizer;
// set up the GUI action
GuiHyperGraphAction guiHyperGraphAction;
guiHyperGraphAction.viewer = mw.viewer;
//optimizer->addPostIterationAction(&guiHyperGraphAction);
optimizer->addPreIterationAction(&guiHyperGraphAction);
if (inputFilename.size() > 0) {
mw.loadFromFile(QString::fromLocal8Bit(inputFilename.c_str()));
}
QCoreApplication* myapp = QApplication::instance();
while (mw.isVisible()) {
guiHyperGraphAction.dumpScreenshots = mw.actionDump_Images->isChecked();
if (myapp)
myapp->processEvents();
SleepThread::msleep(10);
}
delete optimizer;
return 0;
}
const StringWriterPtr RandomService::commandInfo(const CommandArgs& nCommandArgs)
{
StringWriterPtr stringWriter_(new StringWriter());
nCommandArgs.runStringWriter(stringWriter_);
stringWriter_->startClass("result");
string className_("");
__i32 classid_ = __classinfo<RandomService>(className_);
stringWriter_->runString(className_, "className");
stringWriter_->runInt32(classid_, "classId");
stringWriter_->finishClass();
stringWriter_->runClose();
return stringWriter_;
}
int main(int argc, char **argv){
CommandArgs arg;
int nRobots;
int idRobot;
std::string base_addr;
ros::init(argc, argv, "comm_publisher");
arg.param("idRobot", idRobot, 0, "robot identifier" );
arg.param("nRobots", nRobots, 1, "number of robots in the network" );
arg.param("base_addr", base_addr, "127.0.0.", "base network addres");
arg.parseArgs(argc, argv);
CommPublisher cp(idRobot, nRobots, base_addr);
cp.init();
cp.start();
ros::spin();
std::cerr << "Finished" << std::endl;
}
int SelectVersion(const CommandArgs& args)
{
set<int> unitySupportedVersions;
set<int> pluginSupportedVersions;
pluginSupportedVersions.insert(2);
// Read supported versions from unity
CommandArgs::const_iterator i = args.begin();
i += 2;
for ( ; i != args.end(); ++i)
{
unitySupportedVersions.insert(atoi(i->c_str()));
}
set<int> candidates;
set_intersection(unitySupportedVersions.begin(), unitySupportedVersions.end(),
pluginSupportedVersions.begin(), pluginSupportedVersions.end(),
inserter(candidates, candidates.end()));
if (candidates.empty())
return -1;
return *candidates.rbegin();
}
const StringWriterPtr AccountService::commandInfo(const CommandArgs& nCommandArgs)
{
StringWriterPtr stringWriter_(new StringWriter());
nCommandArgs.runStringWriter(stringWriter_);
stringWriter_->startClass("result");
string className_("");
__i32 classid_ = __classinfo<AccountService>(className_);
stringWriter_->runString(className_, "className");
stringWriter_->runInt32(classid_, "classId");
#ifdef __CLIENT__
mAccount->runStringWriter(stringWriter_);
#endif
stringWriter_->finishClass();
stringWriter_->runClose();
return stringWriter_;
}
int main(int argc, char** argv)
{
bool fixLaser;
int maxIterations;
bool verbose;
string inputFilename;
string outputfilename;
string rawFilename;
string odomTestFilename;
string dumpGraphFilename;
// command line parsing
CommandArgs commandLineArguments;
commandLineArguments.param("i", maxIterations, 10, "perform n iterations");
commandLineArguments.param("v", verbose, false, "verbose output of the optimization process");
commandLineArguments.param("o", outputfilename, "", "output final version of the graph");
commandLineArguments.param("test", odomTestFilename, "", "apply odometry calibration to some test data");
commandLineArguments.param("dump", dumpGraphFilename, "", "write the graph to the disk");
commandLineArguments.param("fixLaser", fixLaser, false, "keep the laser offset fixed during optimization");
commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
commandLineArguments.paramLeftOver("raw-log", rawFilename, "", "raw log file containing the odometry");
commandLineArguments.parseArgs(argc, argv);
SparseOptimizer optimizer;
optimizer.setVerbose(verbose);
optimizer.setForceStopFlag(&hasToStop);
allocateSolverForSclam(optimizer);
// loading
if (! Gm2dlIO::readGm2dl(inputFilename, optimizer, false)) {
cerr << "Error while loading gm2dl file" << endl;
}
DataQueue robotLaserQueue;
int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, robotLaserQueue);
if (numLaserOdom == 0) {
cerr << "No raw information read" << endl;
return 0;
}
cerr << "Read " << numLaserOdom << " laser readings from file" << endl;
bool gaugeFreedom = optimizer.gaugeFreedom();
OptimizableGraph::Vertex* gauge = optimizer.findGauge();
if (gaugeFreedom) {
if (! gauge) {
cerr << "# cannot find a vertex to fix in this thing" << endl;
return 2;
} else {
cerr << "# graph is fixed by node " << gauge->id() << endl;
gauge->setFixed(true);
}
} else {
cerr << "# graph is fixed by priors" << endl;
}
addOdometryCalibLinksDifferential(optimizer, robotLaserQueue);
// sanity check
HyperDijkstra d(&optimizer);
UniformCostFunction f;
d.shortestPaths(gauge, &f);
//cerr << PVAR(d.visited().size()) << endl;
if (d.visited().size()!=optimizer.vertices().size()) {
cerr << CL_RED("Warning: d.visited().size() != optimizer.vertices().size()") << endl;
cerr << "visited: " << d.visited().size() << endl;
cerr << "vertices: " << optimizer.vertices().size() << endl;
if (1)
for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
if (d.visited().count(v) == 0) {
cerr << "\t unvisited vertex " << it->first << " " << (void*)v << endl;
v->setFixed(true);
}
}
}
for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->fixed()) {
cerr << "\t fixed vertex " << it->first << endl;
}
}
VertexSE2* laserOffset = dynamic_cast<VertexSE2*>(optimizer.vertex(Gm2dlIO::ID_LASERPOSE));
VertexOdomDifferentialParams* odomParamsVertex = dynamic_cast<VertexOdomDifferentialParams*>(optimizer.vertex(Gm2dlIO::ID_ODOMCALIB));
if (fixLaser) {
cerr << "Fix position of the laser offset" << endl;
laserOffset->setFixed(true);
}
signal(SIGINT, sigquit_handler);
cerr << "Doing full estimation" << endl;
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
cerr << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
int i=optimizer.optimize(maxIterations);
if (maxIterations > 0 && !i){
cerr << "optimize failed, result might be invalid" << endl;
}
if (laserOffset) {
cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl;
}
if (odomParamsVertex) {
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomParamsVertex->estimate().transpose() << endl;
}
cerr << "vertices: " << optimizer.vertices().size() << endl;
cerr << "edges: " << optimizer.edges().size() << endl;
if (dumpGraphFilename.size() > 0) {
cerr << "Writing " << dumpGraphFilename << " ... ";
optimizer.save(dumpGraphFilename.c_str());
cerr << "done." << endl;
}
// optional input of a seperate file for applying the odometry calibration
if (odomTestFilename.size() > 0) {
DataQueue testRobotLaserQueue;
int numTestOdom = Gm2dlIO::readRobotLaser(odomTestFilename, testRobotLaserQueue);
if (numTestOdom == 0) {
cerr << "Unable to read test data" << endl;
} else {
ofstream rawStream("odometry_raw.txt");
ofstream calibratedStream("odometry_calibrated.txt");
const Vector3d& odomCalib = odomParamsVertex->estimate();
RobotLaser* prev = dynamic_cast<RobotLaser*>(testRobotLaserQueue.buffer().begin()->second);
SE2 prevCalibratedPose = prev->odomPose();
for (DataQueue::Buffer::const_iterator it = testRobotLaserQueue.buffer().begin(); it != testRobotLaserQueue.buffer().end(); ++it) {
RobotLaser* cur = dynamic_cast<RobotLaser*>(it->second);
assert(cur);
double dt = cur->timestamp() - prev->timestamp();
SE2 motion = prev->odomPose().inverse() * cur->odomPose();
// convert to velocity measurment
MotionMeasurement motionMeasurement(motion.translation().x(), motion.translation().y(), motion.rotation().angle(), dt);
VelocityMeasurement velocityMeasurement = OdomConvert::convertToVelocity(motionMeasurement);
// apply calibration
VelocityMeasurement calibratedVelocityMeasurment = velocityMeasurement;
calibratedVelocityMeasurment.setVl(odomCalib(0) * calibratedVelocityMeasurment.vl());
calibratedVelocityMeasurment.setVr(odomCalib(1) * calibratedVelocityMeasurment.vr());
MotionMeasurement mm = OdomConvert::convertToMotion(calibratedVelocityMeasurment, odomCalib(2));
// combine calibrated odometry with the previous pose
SE2 remappedOdom;
remappedOdom.fromVector(mm.measurement());
SE2 calOdomPose = prevCalibratedPose * remappedOdom;
// write output
rawStream << prev->odomPose().translation().x() << " " << prev->odomPose().translation().y() << " " << prev->odomPose().rotation().angle() << endl;
calibratedStream << calOdomPose.translation().x() << " " << calOdomPose.translation().y() << " " << calOdomPose.rotation().angle() << endl;
prevCalibratedPose = calOdomPose;
prev = cur;
}
}
}
if (outputfilename.size() > 0) {
Gm2dlIO::updateLaserData(optimizer);
cerr << "Writing " << outputfilename << " ... ";
bool writeStatus = Gm2dlIO::writeGm2dl(outputfilename, optimizer);
cerr << (writeStatus ? "done." : "failed") << endl;
}
return 0;
}
virtual bool Run(P4Task& task, const CommandArgs& args)
{
//ConfigRequest req(args, task.
if (args.size() < 2)
{
string msg = "Perforce plugin got invalid config setting :";
for (CommandArgs::const_iterator i = args.begin(); i != args.end(); ++i) {
msg += " ";
msg += *i;
}
Conn().WarnLine(msg, MAConfig);
Conn().EndResponse();
return true;
}
string key = args[1];
string value = Join(args.begin() + 2, args.end(), " ");
ClearStatus();
string logValue = value;
if (key == "vcPerforcePassword")
logValue = "*";
Conn().Log().Info() << "Got config " << key << " = '" << logValue << "'" << Endl;
// This command actually handles several commands all
// concerning connecting to the perforce server
if (key == "vcPerforceUsername")
{
task.SetP4User(value);
}
else if (key == "vcPerforceWorkspace")
{
task.SetP4Client(value);
}
else if (key == "projectPath")
{
task.SetProjectPath(TrimEnd(value));
Conn().Log().Info() << "Set projectPath to" << value << Endl;
}
else if (key == "vcSharedLogLevel")
{
Conn().Log().Info() << "Set log level to " << value << Endl;
LogLevel level = LOG_DEBUG;
if (value == "info")
level = LOG_INFO;
else if (value == "notice")
level = LOG_NOTICE;
else if (value == "fatal")
level = LOG_FATAL;
Conn().Log().SetLogLevel(level);
}
else if (key == "vcPerforcePassword")
{
task.SetP4Password(value);
value = "*";
}
else if (key == "vcPerforceServer")
{
if (value.empty())
value = "perforce";
string::size_type i = (StartsWith(value, "ssl:") ? value.substr(4) : value).find(":");
if (i == string::npos)
value += ":1666"; // default port
task.SetP4Port(value);
}
else if (key == "vcPerforceHost")
{
task.SetP4Host(value);
}
else if (key == "pluginVersions")
{
int sel = SelectVersion(args);
Conn().DataLine(sel, MAConfig);
Conn().Log().Info() << "Selected plugin protocol version " << sel << Endl;
}
else if (key == "pluginTraits")
{
// We have 4 flags set
Conn().DataLine("6");
Conn().DataLine("requiresNetwork", MAConfig);
Conn().DataLine("enablesCheckout", MAConfig);
Conn().DataLine("enablesLocking", MAConfig);
Conn().DataLine("enablesRevertUnchanged", MAConfig);
Conn().DataLine("enablesChangelists", MAConfig);
Conn().DataLine("enablesGetLatestOnChangeSetSubset", MAConfig);
// We provide 4 configuration fields for the GUI to display
Conn().DataLine("5");
Conn().DataLine("vcPerforceUsername"); // key
Conn().DataLine("Username", MAConfig); // label
Conn().DataLine("The perforce user name", MAConfig); // description
Conn().DataLine(""); // default
Conn().DataLine("1"); // 1 == required field, 2 == password field
Conn().DataLine("vcPerforcePassword");
Conn().DataLine("Password", MAConfig);
Conn().DataLine("The perforce password", MAConfig);
Conn().DataLine("");
Conn().DataLine("2"); // password field
Conn().DataLine("vcPerforceWorkspace");
Conn().DataLine("Workspace", MAConfig);
Conn().DataLine("The perforce workspace/client", MAConfig);
Conn().DataLine("");
Conn().DataLine("1"); // required field
Conn().DataLine("vcPerforceServer");
Conn().DataLine("Server", MAConfig);
Conn().DataLine("The perforce server using format: hostname:port. Port hostname defaults to 'perforce' and port defaults to 1666", MAConfig);
Conn().DataLine("perforce");
Conn().DataLine("0"); //
Conn().DataLine("vcPerforceHost");
Conn().DataLine("Host", MAConfig);
Conn().DataLine("The perforce host ie. P4HOST. It can often be left blank.", MAConfig);
Conn().DataLine("");
Conn().DataLine("0"); //
// We have 11 custom overlay icons
Conn().DataLine("overlays");
Conn().DataLine("11");
Conn().DataLine(IntToString(kLocal)); // for this state
Conn().DataLine("default"); // use this path. "default" and "blank" paths can be used when you have not custom overlays.
Conn().DataLine(IntToString(kOutOfSync));
Conn().DataLine("default");
Conn().DataLine(IntToString(kCheckedOutLocal));
Conn().DataLine("default");
Conn().DataLine(IntToString(kCheckedOutRemote));
Conn().DataLine("default");
Conn().DataLine(IntToString(kDeletedLocal));
Conn().DataLine("default");
Conn().DataLine(IntToString(kDeletedRemote));
Conn().DataLine("default");
Conn().DataLine(IntToString(kAddedLocal));
Conn().DataLine("default");
Conn().DataLine(IntToString(kAddedRemote));
Conn().DataLine("default");
Conn().DataLine(IntToString(kConflicted));
Conn().DataLine("default");
Conn().DataLine(IntToString(kLockedLocal));
Conn().DataLine("default");
Conn().DataLine(IntToString(kLockedRemote));
Conn().DataLine("default");
}
else if (key == "end")
{
task.Logout();
task.Disconnect();
task.DisableUTF8Mode();
}
else
{
Conn().WarnLine(ToString("Unknown config field set on version control plugin: ", key), MAConfig);
}
Conn().EndResponse();
return true;
}
int main(int argc, char **argv)
{
CommandArgs arg;
double resolution;
double maxScore;
double kernelRadius;
int minInliers;
int windowLoopClosure;
double inlierThreshold;
int idRobot;
int nRobots;
std::string outputFilename;
std::string odometryTopic, scanTopic, fixedFrame;
arg.param("resolution", resolution, 0.025, "resolution of the matching grid");
arg.param("maxScore", maxScore, 0.15, "score of the matcher, the higher the less matches");
arg.param("kernelRadius", kernelRadius, 0.2, "radius of the convolution kernel");
arg.param("minInliers", minInliers, 7, "min inliers");
arg.param("windowLoopClosure", windowLoopClosure, 10, "sliding window for loop closures");
arg.param("inlierThreshold", inlierThreshold, 2., "inlier threshold");
arg.param("idRobot", idRobot, 0, "robot identifier" );
arg.param("nRobots", nRobots, 1, "number of robots" );
arg.param("odometryTopic", odometryTopic, "odom", "odometry ROS topic");
arg.param("scanTopic", scanTopic, "scan", "scan ROS topic");
arg.param("fixedFrame", fixedFrame, "odom", "fixed frame to visualize the graph with ROS Rviz");
arg.param("o", outputFilename, "", "file where to save output");
arg.parseArgs(argc, argv);
ros::init(argc, argv, "srslam");
RosHandler rh(idRobot, nRobots, REAL_EXPERIMENT);
rh.setOdomTopic(odometryTopic);
rh.setScanTopic(scanTopic);
rh.useOdom(true);
rh.useLaser(true);
rh.init();
rh.run();
//For estimation
SE2 currEst = rh.getOdom();
std::cout << "My initial position is: " << currEst.translation().x() << " " << currEst.translation().y() << " " << currEst.rotation().angle() << std::endl;
SE2 odomPosk_1 = currEst;
std::cout << "My initial odometry is: " << odomPosk_1.translation().x() << " " << odomPosk_1.translation().y() << " " << odomPosk_1.rotation().angle() << std::endl;
//Graph building
GraphSLAM gslam;
gslam.setIdRobot(idRobot);
int baseId = 10000;
gslam.setBaseId(baseId);
gslam.init(resolution, kernelRadius, windowLoopClosure, maxScore, inlierThreshold, minInliers);
RobotLaser* rlaser = rh.getLaser();
gslam.setInitialData(currEst, odomPosk_1, rlaser);
GraphRosPublisher graphPublisher(gslam.graph(), fixedFrame);
ros::Rate loop_rate(10);
while (ros::ok()){
ros::spinOnce();
SE2 odomPosk = rh.getOdom(); //current odometry
SE2 relodom = odomPosk_1.inverse() * odomPosk;
currEst *= relodom;
odomPosk_1 = odomPosk;
if((distanceSE2(gslam.lastVertex()->estimate(), currEst) > 0.25) ||
(fabs(gslam.lastVertex()->estimate().rotation().angle()-currEst.rotation().angle()) > M_PI_4)){
//Add new data
RobotLaser* laseri = rh.getLaser();
gslam.addDataSM(odomPosk, laseri);
gslam.findConstraints();
struct timeval t_ini, t_fin;
double secs;
gettimeofday(&t_ini, NULL);
gslam.optimize(5);
gettimeofday(&t_fin, NULL);
secs = timeval_diff(&t_fin, &t_ini);
printf("Optimization took %.16g milliseconds\n", secs * 1000.0);
currEst = gslam.lastVertex()->estimate();
char buf[100];
sprintf(buf, "robot-%i-%s", idRobot, outputFilename.c_str());
gslam.saveGraph(buf);
//Publish graph to visualize it on Rviz
graphPublisher.publishGraph();
}
loop_rate.sleep();
}
return 0;
}
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)
{
bool fixLaser;
int maxIterations;
bool verbose;
string inputFilename;
string outputfilename;
string rawFilename;
string odomTestFilename;
string dumpGraphFilename;
// command line parsing
CommandArgs commandLineArguments;
commandLineArguments.param("i", maxIterations, 10, "perform n iterations");
commandLineArguments.param("v", verbose, false, "verbose output of the optimization process");
commandLineArguments.param("o", outputfilename, "", "output final version of the graph");
commandLineArguments.param("test", odomTestFilename, "", "apply odometry calibration to some test data");
commandLineArguments.param("dump", dumpGraphFilename, "", "write the graph to the disk");
commandLineArguments.param("fixLaser", fixLaser, false, "keep the laser offset fixed during optimization");
commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
commandLineArguments.paramLeftOver("raw-log", rawFilename, "", "raw log file containing the odometry");
commandLineArguments.parseArgs(argc, argv);
SparseOptimizer optimizer;
optimizer.setVerbose(verbose);
allocateSolverForSclam(optimizer);
// loading
DataQueue odometryQueue;
int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, odometryQueue);
if (numLaserOdom == 0) {
cerr << "No raw information read" << endl;
return 0;
}
cerr << "Read " << numLaserOdom << " laser readings from file" << endl;
Eigen::Vector3d odomCalib(1., 1., 1.);
SE2 initialLaserPose;
DataQueue robotLaserQueue;
int numRobotLaser = Gm2dlIO::readRobotLaser(inputFilename, robotLaserQueue);
if (numRobotLaser == 0) {
cerr << "No robot laser read" << endl;
return 0;
} else {
RobotLaser* rl = dynamic_cast<RobotLaser*>(robotLaserQueue.buffer().begin()->second);
initialLaserPose = rl->odomPose().inverse() * rl->laserPose();
cerr << PVAR(initialLaserPose.toVector().transpose()) << endl;
}
// adding the measurements
vector<MotionInformation, Eigen::aligned_allocator<MotionInformation> > motions;
{
std::map<double, RobotData*>::const_iterator it = robotLaserQueue.buffer().begin();
std::map<double, RobotData*>::const_iterator prevIt = it++;
for (; it != robotLaserQueue.buffer().end(); ++it) {
MotionInformation mi;
RobotLaser* prevLaser = dynamic_cast<RobotLaser*>(prevIt->second);
RobotLaser* curLaser = dynamic_cast<RobotLaser*>(it->second);
mi.laserMotion = prevLaser->laserPose().inverse() * curLaser->laserPose();
// get the motion of the robot in that time interval
RobotLaser* prevOdom = dynamic_cast<RobotLaser*>(odometryQueue.findClosestData(prevLaser->timestamp()));
RobotLaser* curOdom = dynamic_cast<RobotLaser*>(odometryQueue.findClosestData(curLaser->timestamp()));
mi.odomMotion = prevOdom->odomPose().inverse() * curOdom->odomPose();
mi.timeInterval = prevOdom->timestamp() - curOdom->timestamp();
prevIt = it;
motions.push_back(mi);
}
}
if (1) {
VertexSE2* laserOffset = new VertexSE2;
laserOffset->setId(Gm2dlIO::ID_LASERPOSE);
laserOffset->setEstimate(initialLaserPose);
optimizer.addVertex(laserOffset);
VertexOdomDifferentialParams* odomParamsVertex = new VertexOdomDifferentialParams;
odomParamsVertex->setId(Gm2dlIO::ID_ODOMCALIB);
odomParamsVertex->setEstimate(Eigen::Vector3d(1., 1., 1.));
optimizer.addVertex(odomParamsVertex);
for (size_t i = 0; i < motions.size(); ++i) {
const SE2& odomMotion = motions[i].odomMotion;
const SE2& laserMotion = motions[i].laserMotion;
const double& timeInterval = motions[i].timeInterval;
// add the edge
MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
OdomAndLaserMotion meas;
meas.velocityMeasurement = OdomConvert::convertToVelocity(mm);
meas.laserMotion = laserMotion;
EdgeSE2PureCalib* calibEdge = new EdgeSE2PureCalib;
calibEdge->setVertex(0, laserOffset);
calibEdge->setVertex(1, odomParamsVertex);
calibEdge->setInformation(Eigen::Matrix3d::Identity());
calibEdge->setMeasurement(meas);
if (! optimizer.addEdge(calibEdge)) {
cerr << "Error adding calib edge" << endl;
delete calibEdge;
}
}
if (fixLaser) {
cerr << "Fix position of the laser offset" << endl;
laserOffset->setFixed(true);
}
cerr << "\nPerforming full non-linear estimation" << endl;
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
optimizer.optimize(maxIterations);
cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl;
odomCalib = odomParamsVertex->estimate();
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomParamsVertex->estimate().transpose() << endl;
optimizer.clear();
}
// linear least squares for some parameters
{
Eigen::MatrixXd A(motions.size(), 2);
Eigen::VectorXd x(motions.size());
for (size_t i = 0; i < motions.size(); ++i) {
const SE2& odomMotion = motions[i].odomMotion;
const SE2& laserMotion = motions[i].laserMotion;
const double& timeInterval = motions[i].timeInterval;
MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
VelocityMeasurement velMeas = OdomConvert::convertToVelocity(mm);
A(i, 0) = velMeas.vl() * timeInterval;
A(i, 1) = velMeas.vr() * timeInterval;
x(i) = laserMotion.rotation().angle();
}
//linearSolution = (A.transpose() * A).inverse() * A.transpose() * x;
linearSolution = A.colPivHouseholderQr().solve(x);
//cout << PVAR(linearSolution.transpose()) << endl;
}
//constructing non-linear least squares
VertexSE2* laserOffset = new VertexSE2;
laserOffset->setId(Gm2dlIO::ID_LASERPOSE);
laserOffset->setEstimate(initialLaserPose);
optimizer.addVertex(laserOffset);
VertexBaseline* odomParamsVertex = new VertexBaseline;
odomParamsVertex->setId(Gm2dlIO::ID_ODOMCALIB);
odomParamsVertex->setEstimate(1.);
optimizer.addVertex(odomParamsVertex);
for (size_t i = 0; i < motions.size(); ++i) {
const SE2& odomMotion = motions[i].odomMotion;
const SE2& laserMotion = motions[i].laserMotion;
const double& timeInterval = motions[i].timeInterval;
// add the edge
MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
OdomAndLaserMotion meas;
meas.velocityMeasurement = OdomConvert::convertToVelocity(mm);
meas.laserMotion = laserMotion;
EdgeCalib* calibEdge = new EdgeCalib;
calibEdge->setVertex(0, laserOffset);
calibEdge->setVertex(1, odomParamsVertex);
calibEdge->setInformation(Eigen::Matrix3d::Identity());
calibEdge->setMeasurement(meas);
if (! optimizer.addEdge(calibEdge)) {
cerr << "Error adding calib edge" << endl;
delete calibEdge;
}
}
if (fixLaser) {
cerr << "Fix position of the laser offset" << endl;
laserOffset->setFixed(true);
}
cerr << "\nPerforming partial non-linear estimation" << endl;
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
optimizer.optimize(maxIterations);
cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl;
odomCalib(0) = -1. * linearSolution(0) * odomParamsVertex->estimate();
odomCalib(1) = linearSolution(1) * odomParamsVertex->estimate();
odomCalib(2) = odomParamsVertex->estimate();
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomCalib.transpose() << endl;
{
SE2 closedFormLaser;
Eigen::Vector3d closedFormOdom;
ClosedFormCalibration::calibrate(motions, closedFormLaser, closedFormOdom);
cerr << "\nObtaining closed form solution" << endl;
cerr << "Calibrated laser offset (x, y, theta):" << closedFormLaser.toVector().transpose() << endl;
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << closedFormOdom.transpose() << endl;
}
if (dumpGraphFilename.size() > 0) {
cerr << "Writing " << dumpGraphFilename << " ... ";
optimizer.save(dumpGraphFilename.c_str());
cerr << "done." << endl;
}
// optional input of a separate file for applying the odometry calibration
if (odomTestFilename.size() > 0) {
DataQueue testRobotLaserQueue;
int numTestOdom = Gm2dlIO::readRobotLaser(odomTestFilename, testRobotLaserQueue);
if (numTestOdom == 0) {
cerr << "Unable to read test data" << endl;
} else {
ofstream rawStream("odometry_raw.txt");
ofstream calibratedStream("odometry_calibrated.txt");
RobotLaser* prev = dynamic_cast<RobotLaser*>(testRobotLaserQueue.buffer().begin()->second);
SE2 prevCalibratedPose = prev->odomPose();
for (DataQueue::Buffer::const_iterator it = testRobotLaserQueue.buffer().begin(); it != testRobotLaserQueue.buffer().end(); ++it) {
RobotLaser* cur = dynamic_cast<RobotLaser*>(it->second);
assert(cur);
double dt = cur->timestamp() - prev->timestamp();
SE2 motion = prev->odomPose().inverse() * cur->odomPose();
// convert to velocity measurement
MotionMeasurement motionMeasurement(motion.translation().x(), motion.translation().y(), motion.rotation().angle(), dt);
VelocityMeasurement velocityMeasurement = OdomConvert::convertToVelocity(motionMeasurement);
// apply calibration
VelocityMeasurement calibratedVelocityMeasurment = velocityMeasurement;
calibratedVelocityMeasurment.setVl(odomCalib(0) * calibratedVelocityMeasurment.vl());
calibratedVelocityMeasurment.setVr(odomCalib(1) * calibratedVelocityMeasurment.vr());
MotionMeasurement mm = OdomConvert::convertToMotion(calibratedVelocityMeasurment, odomCalib(2));
// combine calibrated odometry with the previous pose
SE2 remappedOdom;
remappedOdom.fromVector(mm.measurement());
SE2 calOdomPose = prevCalibratedPose * remappedOdom;
// write output
rawStream << prev->odomPose().translation().x() << " " << prev->odomPose().translation().y() << " " << prev->odomPose().rotation().angle() << endl;
calibratedStream << calOdomPose.translation().x() << " " << calOdomPose.translation().y() << " " << calOdomPose.rotation().angle() << endl;
prevCalibratedPose = calOdomPose;
prev = cur;
}
}
}
return 0;
}
int main(int argc, char** argv) {
CommandArgs arg;
int nlandmarks;
int nSegments;
int simSteps;
double worldSize;
bool hasOdom;
bool hasPoseSensor;
bool hasPointSensor;
bool hasPointBearingSensor;
bool hasSegmentSensor;
bool hasCompass;
bool hasGPS;
int segmentGridSize;
double minSegmentLenght, maxSegmentLenght;
std::string outputFilename;
arg.param("simSteps", simSteps, 100, "number of simulation steps");
arg.param("nLandmarks", nlandmarks, 1000, "number of landmarks");
arg.param("nSegments", nSegments, 1000, "number of segments");
arg.param("segmentGridSize", segmentGridSize, 50, "number of cells of the grid where to align the segments");
arg.param("minSegmentLenght", minSegmentLenght, 0.5, "minimal lenght of a segment in the world");
arg.param("maxSegmentLenght", maxSegmentLenght, 3, "maximal lenght of a segment in the world");
arg.param("worldSize", worldSize, 25.0, "size of the world");
arg.param("hasOdom", hasOdom, false, "the robot has an odometry" );
arg.param("hasPointSensor", hasPointSensor, false, "the robot has a point sensor" );
arg.param("hasPointBearingSensor", hasPointBearingSensor, false, "the robot has a point bearing sensor" );
arg.param("hasPoseSensor", hasPoseSensor, false, "the robot has a pose sensor" );
arg.param("hasCompass", hasCompass, false, "the robot has a compass");
arg.param("hasGPS", hasGPS, false, "the robot has a GPS");
arg.param("hasSegmentSensor", hasSegmentSensor, false, "the robot has a segment sensor" );
arg.paramLeftOver("graph-output", outputFilename, "simulator_out.g2o", "graph file which will be written", true);
arg.parseArgs(argc, argv);
std::tr1::ranlux_base_01 generator;
OptimizableGraph graph;
World world(&graph);
for (int i=0; i<nlandmarks; i++){
WorldObjectPointXY * landmark = new WorldObjectPointXY;
double x = sampleUniform(-.5,.5, &generator)*worldSize;
double y = sampleUniform(-.5,.5, &generator)*worldSize;
landmark->vertex()->setEstimate(Vector2d(x,y));
world.addWorldObject(landmark);
}
cerr << "nSegments = " << nSegments << endl;
for (int i=0; i<nSegments; i++){
WorldObjectSegment2D * segment = new WorldObjectSegment2D;
int ix = sampleUniform(-segmentGridSize,segmentGridSize, &generator);
int iy = sampleUniform(-segmentGridSize,segmentGridSize, &generator);
int ith = sampleUniform(0,3, &generator);
double th= (M_PI/2)*ith;
th=atan2(sin(th),cos(th));
double xc = ix*(worldSize/segmentGridSize);
double yc = iy*(worldSize/segmentGridSize);
double l2 = sampleUniform(minSegmentLenght, maxSegmentLenght, &generator);
double x1 = xc + cos(th)*l2;
double y1 = yc + sin(th)*l2;
double x2 = xc - cos(th)*l2;
double y2 = yc - sin(th)*l2;
segment->vertex()->setEstimateP1(Vector2d(x1,y1));
segment->vertex()->setEstimateP2(Vector2d(x2,y2));
world.addWorldObject(segment);
}
Robot2D robot(&world, "myRobot");
world.addRobot(&robot);
stringstream ss;
ss << "-ws" << worldSize;
ss << "-nl" << nlandmarks;
ss << "-steps" << simSteps;
if (hasOdom) {
SensorOdometry2D* odometrySensor=new SensorOdometry2D("odometry");
robot.addSensor(odometrySensor);
Matrix3d odomInfo = odometrySensor->information();
odomInfo.setIdentity();
odomInfo*=100;
odomInfo(2,2)=1000;
odometrySensor->setInformation(odomInfo);
ss << "-odom";
}
if (hasPoseSensor) {
SensorPose2D* poseSensor = new SensorPose2D("poseSensor");
robot.addSensor(poseSensor);
Matrix3d poseInfo = poseSensor->information();
poseInfo.setIdentity();
poseInfo*=100;
poseInfo(2,2)=1000;
poseSensor->setInformation(poseInfo);
ss << "-pose";
}
if (hasPointSensor) {
SensorPointXY* pointSensor = new SensorPointXY("pointSensor");
robot.addSensor(pointSensor);
Matrix2d pointInfo = pointSensor->information();
pointInfo.setIdentity();
pointInfo*=100;
pointSensor->setInformation(pointInfo);
ss << "-pointXY";
}
if (hasPointBearingSensor) {
SensorPointXYBearing* bearingSensor = new SensorPointXYBearing("bearingSensor");
robot.addSensor(bearingSensor);
bearingSensor->setInformation(bearingSensor->information()*1000);
ss << "-pointBearing";
}
if (hasSegmentSensor) {
SensorSegment2D* segmentSensor = new SensorSegment2D("segmentSensorSensor");
segmentSensor->setMaxRange(3);
segmentSensor->setMinRange(.1);
robot.addSensor(segmentSensor);
segmentSensor->setInformation(segmentSensor->information()*1000);
SensorSegment2DLine* segmentSensorLine = new SensorSegment2DLine("segmentSensorSensorLine");
segmentSensorLine->setMaxRange(3);
segmentSensorLine->setMinRange(.1);
robot.addSensor(segmentSensorLine);
Matrix2d m=segmentSensorLine->information();
m=m*1000;
m(0,0)*=10;
segmentSensorLine->setInformation(m);
SensorSegment2DPointLine* segmentSensorPointLine = new SensorSegment2DPointLine("segmentSensorSensorPointLine");
segmentSensorPointLine->setMaxRange(3);
segmentSensorPointLine->setMinRange(.1);
robot.addSensor(segmentSensorPointLine);
Matrix3d m3=segmentSensorPointLine->information();
m3=m3*1000;
m3(3,3)*=10;
segmentSensorPointLine->setInformation(m3);
ss << "-segment2d";
}
robot.move(SE2());
double pStraight=0.7;
SE2 moveStraight; moveStraight.setTranslation(Vector2d(1., 0.));
double pLeft=0.15;
SE2 moveLeft; moveLeft.setRotation(Rotation2Dd(M_PI/2));
//double pRight=0.15;
SE2 moveRight; moveRight.setRotation(Rotation2Dd(-M_PI/2));
for (int i=0; i<simSteps; i++){
cerr << "m";
SE2 move;
SE2 pose=robot.pose();
double dtheta=-100;
Vector2d dt;
if (pose.translation().x() < -.5*worldSize){
dtheta = 0;
}
if (pose.translation().x() > .5*worldSize){
dtheta = -M_PI;
}
if (pose.translation().y() < -.5*worldSize){
dtheta = M_PI/2;
}
if (pose.translation().y() > .5*worldSize){
dtheta = -M_PI/2;
}
if (dtheta< -M_PI) {
// select a random move of the robot
double sampled=sampleUniform(0.,1.,&generator);
if (sampled<pStraight)
move=moveStraight;
else if (sampled<pStraight+pLeft)
move=moveLeft;
else
move=moveRight;
} else {
double mTheta=dtheta-pose.rotation().angle();
move.setRotation(Rotation2Dd(mTheta));
if (move.rotation().angle()<std::numeric_limits<double>::epsilon()){
move.setTranslation(Vector2d(1., 0.));
}
}
robot.relativeMove(move);
// do a sense
cerr << "s";
robot.sense();
}
//string fname=outputFilename + ss.str() + ".g2o";
ofstream testStream(outputFilename.c_str());
graph.save(testStream);
return 0;
}
virtual bool Run(P4Task& task, const CommandArgs& args)
{
ClearStatus();
Conn().Log().Info() << args[0] << "::Run()" << Endl;
bool noLocalFileMove = args.size() > 1 && args[1] == "noLocalFileMove";
VersionedAssetList assetList;
Conn() >> assetList;
if ( assetList.empty() )
{
Conn().EndResponse();
return true;
}
// Process two assets at a time ie. src,dest
if ( assetList.size() % 2 )
{
Conn().WarnLine("uneven number of assets during move", MASystem);
Conn().EndResponse();
return true;
}
VersionedAssetList::const_iterator b = assetList.begin();
VersionedAssetList::const_iterator e = b;
// Split into two steps. 1st make everything editable and 2nd do the move.
// this makes changes more atomic.
while (b != assetList.end())
{
e += 2;
const VersionedAsset& src = *b;
string paths = ResolvePaths(b, e, kPathWild | kPathRecursive);
Conn().Log().Debug() << "Ensure editable source " << paths << Endl;
string err;
bool editable = (src.GetState() & (kCheckedOutLocal | kAddedLocal | kLockedLocal)) != 0;
if (!editable)
{
string srcPath = ResolvePaths(b, b+1, kPathWild | kPathRecursive);
Conn().Log().Info() << "edit " << srcPath << Endl;
if (!task.CommandRun("edit " + srcPath, this))
{
break;
}
}
b = e;
}
b = assetList.begin();
e = b;
VersionedAssetList targetAssetList;
string noLocalFileMoveFlag = noLocalFileMove ? " -k " : "";
while (!HasErrors() && b != assetList.end())
{
e += 2;
const VersionedAsset& src = *b;
const VersionedAsset& dest = *(b+1);
targetAssetList.push_back(dest);
string paths = ResolvePaths(b, e, kPathWild | kPathRecursive);
Conn().Log().Info() << "move " << noLocalFileMoveFlag << paths << Endl;
if (!task.CommandRun("move " + noLocalFileMoveFlag + paths, this))
{
break;
}
// Make the actual file system move if perforce didn't do it ie. in
// the case of an empty folder rename or a non versioned asset/folder move/rename
if (!PathExists(dest.GetPath()))
{
// Move the file
if (!MoveAFile(src.GetPath(), dest.GetPath()))
{
string errorMessage = "Error moving file ";
errorMessage += src.GetPath();
errorMessage += " to ";
errorMessage += dest.GetPath();
Conn().WarnLine(errorMessage);
}
}
// Delete move folder src since perforce leaves around empty folders.
// This only works because unity will not send embedded moves.
if (src.IsFolder() && IsDirectory(src.GetPath()))
{
DeleteRecursive(src.GetPath());
}
b = e;
}
// We just wrap up the communication here.
Conn() << GetStatus();
RunAndSendStatus(task, targetAssetList);
Conn().EndResponse();
return true;
}
int main(int argc, char** argv)
{
string inputFilename;
string outputFilename;
// command line parsing
CommandArgs commandLineArguments;
commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
commandLineArguments.paramLeftOver("gm2dl-output", outputFilename, "", "name of the output file");
commandLineArguments.parseArgs(argc, argv);
OptimizableGraph inputGraph;
bool loadStatus = inputGraph.load(inputFilename.c_str());
if (! loadStatus) {
cerr << "Error while loading input data" << endl;
return 1;
}
OptimizableGraph outputGraph;
// process all the vertices first
double fx = -1;
double baseline = -1;
bool firstCam = true;
for (OptimizableGraph::VertexIDMap::const_iterator it = inputGraph.vertices().begin(); it != inputGraph.vertices().end(); ++it) {
if (dynamic_cast<VertexCam*>(it->second)) {
VertexCam* v = static_cast<VertexCam*>(it->second);
if (firstCam) {
firstCam = false;
g2o::ParameterCamera* camParams = new g2o::ParameterCamera;
camParams->setId(0);
const SBACam& c = v->estimate();
baseline = c.baseline;
fx = c.Kcam(0,0);
camParams->setKcam(c.Kcam(0,0), c.Kcam(1,1), c.Kcam(0,2), c.Kcam(1,2));
outputGraph.addParameter(camParams);
}
VertexSE3* ov = new VertexSE3;
ov->setId(v->id());
Eigen::Isometry3d p;
p = v->estimate().rotation();
p.translation() = v->estimate().translation();
ov->setEstimate(p);
if (! outputGraph.addVertex(ov)) {
assert(0 && "Failure adding camera vertex");
}
}
else if (dynamic_cast<VertexSBAPointXYZ*>(it->second)) {
VertexSBAPointXYZ* v = static_cast<VertexSBAPointXYZ*>(it->second);
VertexPointXYZ* ov = new VertexPointXYZ;
ov->setId(v->id());
ov->setEstimate(v->estimate());
if (! outputGraph.addVertex(ov)) {
assert(0 && "Failure adding camera vertex");
}
}
}
for (OptimizableGraph::EdgeSet::const_iterator it = inputGraph.edges().begin(); it != inputGraph.edges().end(); ++it) {
if (dynamic_cast<EdgeProjectP2SC*>(*it)) {
EdgeProjectP2SC* e = static_cast<EdgeProjectP2SC*>(*it);
EdgeSE3PointXYZDisparity* oe = new EdgeSE3PointXYZDisparity;
oe->vertices()[0] = outputGraph.vertex(e->vertices()[1]->id());
oe->vertices()[1] = outputGraph.vertex(e->vertices()[0]->id());
double kx = e->measurement().x();
double ky = e->measurement().y();
double disparity = kx - e->measurement()(2);
oe->setMeasurement(Eigen::Vector3d(kx, ky, disparity / (fx * baseline)));
oe->setInformation(e->information()); // TODO convert information matrix
oe->setParameterId(0,0);
if (! outputGraph.addEdge(oe)) {
assert(0 && "error adding edge");
}
}
}
cout << "Vertices in/out:\t" << inputGraph.vertices().size() << " " << outputGraph.vertices().size() << endl;
cout << "Edges in/out:\t" << inputGraph.edges().size() << " " << outputGraph.edges().size() << endl;
cout << "Writing output ... " << flush;
outputGraph.save(outputFilename.c_str());
cout << "done." << endl;
return 0;
}
int main(int argc, char** argv)
{
OptimizableGraph::initMultiThreading();
int maxIterations;
bool verbose;
string inputFilename;
string gnudump;
string outputfilename;
string solverProperties;
string strSolver;
string loadLookup;
bool initialGuess;
bool initialGuessOdometry;
bool marginalize;
bool listTypes;
bool listSolvers;
bool listRobustKernels;
bool incremental;
bool guiOut;
int gaugeId;
string robustKernel;
bool computeMarginals;
bool printSolverProperties;
double huberWidth;
double gain;
int maxIterationsWithGain;
//double lambdaInit;
int updateGraphEachN = 10;
string statsFile;
string summaryFile;
bool nonSequential;
// command line parsing
std::vector<int> gaugeList;
CommandArgs arg;
arg.param("i", maxIterations, 5, "perform n iterations, if negative consider the gain");
arg.param("gain", gain, 1e-6, "the gain used to stop optimization (default = 1e-6)");
arg.param("ig",maxIterationsWithGain, std::numeric_limits<int>::max(), "Maximum number of iterations with gain enabled (default: inf)");
arg.param("v", verbose, false, "verbose output of the optimization process");
arg.param("guess", initialGuess, false, "initial guess based on spanning tree");
arg.param("guessOdometry", initialGuessOdometry, false, "initial guess based on odometry");
arg.param("inc", incremental, false, "run incremetally");
arg.param("update", updateGraphEachN, 10, "updates after x odometry nodes");
arg.param("guiout", guiOut, false, "gui output while running incrementally");
arg.param("marginalize", marginalize, false, "on or off");
arg.param("printSolverProperties", printSolverProperties, false, "print the properties of the solver");
arg.param("solverProperties", solverProperties, "", "set the internal properties of a solver,\n\te.g., initialLambda=0.0001,maxTrialsAfterFailure=2");
arg.param("gnudump", gnudump, "", "dump to gnuplot data file");
arg.param("robustKernel", robustKernel, "", "use this robust error function");
arg.param("robustKernelWidth", huberWidth, -1., "width for the robust Kernel (only if robustKernel)");
arg.param("computeMarginals", computeMarginals, false, "computes the marginal covariances of something. FOR TESTING ONLY");
arg.param("gaugeId", gaugeId, -1, "force the gauge");
arg.param("o", outputfilename, "", "output final version of the graph");
arg.param("solver", strSolver, "gn_var", "specify which solver to use underneat\n\t {gn_var, lm_fix3_2, gn_fix6_3, lm_fix7_3}");
#ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES
string dummy;
arg.param("solverlib", dummy, "", "specify a solver library which will be loaded");
arg.param("typeslib", dummy, "", "specify a types library which will be loaded");
#endif
arg.param("stats", statsFile, "", "specify a file for the statistics");
arg.param("listTypes", listTypes, false, "list the registered types");
arg.param("listRobustKernels", listRobustKernels, false, "list the registered robust kernels");
arg.param("listSolvers", listSolvers, false, "list the available solvers");
arg.param("renameTypes", loadLookup, "", "create a lookup for loading types into other types,\n\t TAG_IN_FILE=INTERNAL_TAG_FOR_TYPE,TAG2=INTERNAL2\n\t e.g., VERTEX_CAM=VERTEX_SE3:EXPMAP");
arg.param("gaugeList", gaugeList, std::vector<int>(), "set the list of gauges separated by commas without spaces \n e.g: 1,2,3,4,5 ");
arg.param("summary", summaryFile, "", "append a summary of this optimization run to the summary file passed as argument");
arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);
arg.param("nonSequential", nonSequential, false, "apply the robust kernel only on loop closures and not odometries");
arg.parseArgs(argc, argv);
if (verbose) {
cout << "# Used Compiler: " << G2O_CXX_COMPILER << endl;
}
#ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES
// registering all the types from the libraries
DlWrapper dlTypesWrapper;
loadStandardTypes(dlTypesWrapper, argc, argv);
// register all the solvers
DlWrapper dlSolverWrapper;
loadStandardSolver(dlSolverWrapper, argc, argv);
#else
if (verbose)
cout << "# linked version of g2o" << endl;
#endif
OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
if (listSolvers) {
solverFactory->listSolvers(cout);
}
if (listTypes) {
Factory::instance()->printRegisteredTypes(cout, true);
}
if (listRobustKernels) {
std::vector<std::string> kernels;
RobustKernelFactory::instance()->fillKnownKernels(kernels);
cout << "Robust Kernels:" << endl;
for (size_t i = 0; i < kernels.size(); ++i) {
cout << kernels[i] << endl;
}
}
SparseOptimizer optimizer;
optimizer.setVerbose(verbose);
optimizer.setForceStopFlag(&hasToStop);
SparseOptimizerTerminateAction* terminateAction = 0;
if (maxIterations < 0) {
cerr << "# setup termination criterion based on the gain of the iteration" << endl;
maxIterations = maxIterationsWithGain;
terminateAction = new SparseOptimizerTerminateAction;
terminateAction->setGainThreshold(gain);
terminateAction->setMaxIterations(maxIterationsWithGain);
optimizer.addPostIterationAction(terminateAction);
}
// allocating the desired solver + testing whether the solver is okay
OptimizationAlgorithmProperty solverProperty;
optimizer.setAlgorithm(solverFactory->construct(strSolver, solverProperty));
if (! optimizer.solver()) {
cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl;
return 0;
}
if (solverProperties.size() > 0) {
bool updateStatus = optimizer.solver()->updatePropertiesFromString(solverProperties);
if (! updateStatus) {
cerr << "Failure while updating the solver properties from the given string" << endl;
}
}
if (solverProperties.size() > 0 || printSolverProperties) {
optimizer.solver()->printProperties(cerr);
}
// Loading the input data
if (loadLookup.size() > 0) {
optimizer.setRenamedTypesFromString(loadLookup);
}
if (inputFilename.size() == 0) {
cerr << "No input data specified" << endl;
return 0;
} else if (inputFilename == "-") {
cerr << "Read input from stdin" << endl;
if (!optimizer.load(cin)) {
cerr << "Error loading graph" << endl;
return 2;
}
} else {
cerr << "Read input from " << inputFilename << endl;
ifstream ifs(inputFilename.c_str());
if (!ifs) {
cerr << "Failed to open file" << endl;
return 1;
}
if (!optimizer.load(ifs)) {
cerr << "Error loading graph" << endl;
return 2;
}
}
cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl;
cerr << "Loaded " << optimizer.edges().size() << " edges" << endl;
if (optimizer.vertices().size() == 0) {
cerr << "Graph contains no vertices" << endl;
return 1;
}
set<int> vertexDimensions = optimizer.dimensions();
if (! optimizer.isSolverSuitable(solverProperty, vertexDimensions)) {
cerr << "The selected solver is not suitable for optimizing the given graph" << endl;
return 3;
}
assert (optimizer.solver());
//optimizer.setMethod(str2method(strMethod));
//optimizer.setUserLambdaInit(lambdaInit);
// check for vertices to fix to remove DoF
bool gaugeFreedom = optimizer.gaugeFreedom();
OptimizableGraph::Vertex* gauge=0;
if (gaugeList.size()){
cerr << "Fixing gauges: ";
for (size_t i=0; i<gaugeList.size(); i++){
int id=gaugeList[i];
OptimizableGraph::Vertex* v=optimizer.vertex(id);
if (!v){
cerr << "fatal, not found the vertex of id " << id << " in the gaugeList. Aborting";
return -1;
} else {
if (i==0)
gauge = v;
cerr << v->id() << " ";
v->setFixed(1);
}
}
cerr << endl;
gaugeFreedom = false;
} else {
gauge=optimizer.findGauge();
}
if (gaugeFreedom) {
if (! gauge) {
cerr << "# cannot find a vertex to fix in this thing" << endl;
return 2;
} else {
cerr << "# graph is fixed by node " << gauge->id() << endl;
gauge->setFixed(true);
}
} else {
cerr << "# graph is fixed by priors or already fixed vertex" << endl;
}
// if schur, we wanna marginalize the landmarks...
if (marginalize || solverProperty.requiresMarginalize) {
int maxDim = *vertexDimensions.rbegin();
int minDim = *vertexDimensions.begin();
if (maxDim != minDim) {
cerr << "# Preparing Marginalization of the Landmarks ... ";
for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->dimension() != maxDim) {
v->setMarginalized(true);
}
}
cerr << "done." << endl;
}
}
if (robustKernel.size() > 0) {
AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(robustKernel);
cerr << "# Preparing robust error function ... ";
if (creator) {
if (nonSequential) {
for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
if (e->vertices().size() >= 2 && std::abs(e->vertex(0)->id() - e->vertex(1)->id()) != 1) {
e->setRobustKernel(creator->construct());
if (huberWidth > 0)
e->robustKernel()->setDelta(huberWidth);
}
}
} else {
for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
e->setRobustKernel(creator->construct());
if (huberWidth > 0)
e->robustKernel()->setDelta(huberWidth);
}
}
cerr << "done." << endl;
} else {
cerr << "Unknown Robust Kernel: " << robustKernel << endl;
}
}
// sanity check
HyperDijkstra d(&optimizer);
UniformCostFunction f;
d.shortestPaths(gauge,&f);
//cerr << PVAR(d.visited().size()) << endl;
if (d.visited().size()!=optimizer.vertices().size()) {
cerr << CL_RED("Warning: d.visited().size() != optimizer.vertices().size()") << endl;
cerr << "visited: " << d.visited().size() << endl;
cerr << "vertices: " << optimizer.vertices().size() << endl;
}
if (incremental) {
cerr << CL_RED("# Note: this variant performs batch steps in each time step") << endl;
cerr << CL_RED("# For a variant which updates the Cholesky factor use the binary g2o_incremental") << endl;
int incIterations = maxIterations;
if (! arg.parsedParam("i")) {
cerr << "# Setting default number of iterations" << endl;
incIterations = 1;
}
int updateDisplayEveryN = updateGraphEachN;
int maxDim = 0;
cerr << "# incremental settings" << endl;
cerr << "#\t solve every " << updateGraphEachN << endl;
cerr << "#\t iterations " << incIterations << endl;
SparseOptimizer::VertexIDMap vertices = optimizer.vertices();
for (SparseOptimizer::VertexIDMap::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
const SparseOptimizer::Vertex* v = static_cast<const SparseOptimizer::Vertex*>(it->second);
maxDim = max(maxDim, v->dimension());
}
vector<SparseOptimizer::Edge*> edges;
for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
edges.push_back(e);
}
optimizer.edges().clear();
optimizer.vertices().clear();
optimizer.setVerbose(false);
// sort the edges in a way that inserting them makes sense
sort(edges.begin(), edges.end(), IncrementalEdgesCompare());
double cumTime = 0.;
int vertexCount=0;
int lastOptimizedVertexCount = 0;
int lastVisUpdateVertexCount = 0;
bool freshlyOptimized=false;
bool firstRound = true;
HyperGraph::VertexSet verticesAdded;
HyperGraph::EdgeSet edgesAdded;
for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin(); it != edges.end(); ++it) {
SparseOptimizer::Edge* e = *it;
int doInit = 0;
SparseOptimizer::Vertex* v1 = optimizer.vertex(e->vertices()[0]->id());
SparseOptimizer::Vertex* v2 = optimizer.vertex(e->vertices()[1]->id());
if (! v1) {
SparseOptimizer::Vertex* v = v1 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[0]);
bool v1Added = optimizer.addVertex(v);
//cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
assert(v1Added);
if (! v1Added)
cerr << "Error adding vertex " << v->id() << endl;
else
verticesAdded.insert(v);
doInit = 1;
if (v->dimension() == maxDim)
vertexCount++;
}
if (! v2) {
SparseOptimizer::Vertex* v = v2 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[1]);
bool v2Added = optimizer.addVertex(v);
//cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
assert(v2Added);
if (! v2Added)
cerr << "Error adding vertex " << v->id() << endl;
else
verticesAdded.insert(v);
doInit = 2;
if (v->dimension() == maxDim)
vertexCount++;
}
// adding the edge and initialization of the vertices
{
//cerr << " adding edge " << e->vertices()[0]->id() << " " << e->vertices()[1]->id() << endl;
if (! optimizer.addEdge(e)) {
cerr << "Unable to add edge " << e->vertices()[0]->id() << " -> " << e->vertices()[1]->id() << endl;
} else {
edgesAdded.insert(e);
}
if (doInit) {
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
switch (doInit){
case 1: // initialize v1 from v2
{
HyperGraph::VertexSet toSet;
toSet.insert(to);
if (e->initialEstimatePossible(toSet, from) > 0.) {
//cerr << "init: "
//<< to->id() << "(" << to->dimension() << ") -> "
//<< from->id() << "(" << from->dimension() << ") " << endl;
e->initialEstimate(toSet, from);
} else {
assert(0 && "Added unitialized variable to the graph");
}
break;
}
case 2:
{
HyperGraph::VertexSet fromSet;
fromSet.insert(from);
if (e->initialEstimatePossible(fromSet, to) > 0.) {
//cerr << "init: "
//<< from->id() << "(" << from->dimension() << ") -> "
//<< to->id() << "(" << to->dimension() << ") " << endl;
e->initialEstimate(fromSet, to);
} else {
assert(0 && "Added unitialized variable to the graph");
}
break;
}
default: cerr << "doInit wrong value\n";
}
}
}
freshlyOptimized=false;
{
//cerr << "Optimize" << endl;
if (vertexCount - lastOptimizedVertexCount >= updateGraphEachN) {
if (firstRound) {
if (!optimizer.initializeOptimization()){
cerr << "initialization failed" << endl;
return 0;
}
} else {
if (! optimizer.updateInitialization(verticesAdded, edgesAdded)) {
cerr << "updating initialization failed" << endl;
return 0;
}
}
verticesAdded.clear();
edgesAdded.clear();
double ts = get_monotonic_time();
int currentIt=optimizer.optimize(incIterations, !firstRound);
double dts = get_monotonic_time() - ts;
cumTime += dts;
firstRound = false;
//optimizer->setOptimizationTime(cumTime);
if (verbose) {
double chi2 = optimizer.chi2();
cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2
<< "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl;
}
lastOptimizedVertexCount = vertexCount;
freshlyOptimized = true;
if (guiOut) {
if (vertexCount - lastVisUpdateVertexCount >= updateDisplayEveryN) {
dumpEdges(cout, optimizer);
lastVisUpdateVertexCount = vertexCount;
}
}
}
if (! verbose)
cerr << ".";
}
} // for all edges
if (! freshlyOptimized) {
double ts = get_monotonic_time();
int currentIt=optimizer.optimize(incIterations, !firstRound);
double dts = get_monotonic_time() - ts;
cumTime += dts;
//optimizer->setOptimizationTime(cumTime);
if (verbose) {
double chi2 = optimizer.chi2();
cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2
<< "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl;
}
}
} else {
// BATCH optimization
if (statsFile!=""){
// allocate buffer for statistics;
optimizer.setComputeBatchStatistics(true);
}
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
double loadChi = optimizer.chi2();
cerr << "Initial chi2 = " << FIXED(loadChi) << endl;
if (initialGuess) {
optimizer.computeInitialGuess();
} else if (initialGuessOdometry) {
EstimatePropagatorCostOdometry costFunction(&optimizer);
optimizer.computeInitialGuess(costFunction);
}
double initChi = optimizer.chi2();
signal(SIGINT, sigquit_handler);
int result=optimizer.optimize(maxIterations);
if (maxIterations > 0 && result==OptimizationAlgorithm::Fail){
cerr << "Cholesky failed, result might be invalid" << endl;
} else if (computeMarginals){
std::vector<std::pair<int, int> > blockIndices;
for (size_t i=0; i<optimizer.activeVertices().size(); i++){
OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
if (v->hessianIndex()>=0){
blockIndices.push_back(make_pair(v->hessianIndex(), v->hessianIndex()));
}
if (v->hessianIndex()>0){
blockIndices.push_back(make_pair(v->hessianIndex()-1, v->hessianIndex()));
}
}
SparseBlockMatrix<MatrixXd> spinv;
if (optimizer.computeMarginals(spinv, blockIndices)) {
for (size_t i=0; i<optimizer.activeVertices().size(); i++){
OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
cerr << "Vertex id:" << v->id() << endl;
if (v->hessianIndex()>=0){
cerr << "inv block :" << v->hessianIndex() << ", " << v->hessianIndex()<< endl;
cerr << *(spinv.block(v->hessianIndex(), v->hessianIndex()));
cerr << endl;
}
if (v->hessianIndex()>0){
cerr << "inv block :" << v->hessianIndex()-1 << ", " << v->hessianIndex()<< endl;
cerr << *(spinv.block(v->hessianIndex()-1, v->hessianIndex()));
cerr << endl;
}
}
}
}
optimizer.computeActiveErrors();
double finalChi=optimizer.chi2();
if (summaryFile!="") {
PropertyMap summary;
summary.makeProperty<StringProperty>("filename", inputFilename);
summary.makeProperty<IntProperty>("n_vertices", optimizer.vertices().size());
summary.makeProperty<IntProperty>("n_edges", optimizer.edges().size());
int nLandmarks=0;
int nPoses=0;
int maxDim = *vertexDimensions.rbegin();
for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->dimension() != maxDim) {
nLandmarks++;
} else
nPoses++;
}
set<string> edgeTypes;
for (HyperGraph::EdgeSet::iterator it=optimizer.edges().begin(); it!=optimizer.edges().end(); it++){
edgeTypes.insert(Factory::instance()->tag(*it));
}
stringstream edgeTypesString;
for (std::set<string>::iterator it=edgeTypes.begin(); it!=edgeTypes.end(); it++){
edgeTypesString << *it << " ";
}
summary.makeProperty<IntProperty>("n_poses", nPoses);
summary.makeProperty<IntProperty>("n_landmarks", nLandmarks);
summary.makeProperty<StringProperty>("edge_types", edgeTypesString.str());
summary.makeProperty<DoubleProperty>("load_chi", loadChi);
summary.makeProperty<StringProperty>("solver", strSolver);
summary.makeProperty<BoolProperty>("robustKernel", robustKernel.size() > 0);
summary.makeProperty<DoubleProperty>("init_chi", initChi);
summary.makeProperty<DoubleProperty>("final_chi", finalChi);
summary.makeProperty<IntProperty>("maxIterations", maxIterations);
summary.makeProperty<IntProperty>("realIterations", result);
ofstream os;
os.open(summaryFile.c_str(), ios::app);
summary.writeToCSV(os);
}
if (statsFile!=""){
cerr << "writing stats to file \"" << statsFile << "\" ... ";
ofstream os(statsFile.c_str());
const BatchStatisticsContainer& bsc = optimizer.batchStatistics();
for (int i=0; i<maxIterations; i++) {
os << bsc[i] << endl;
}
cerr << "done." << endl;
}
}
// saving again
if (gnudump.size() > 0) {
bool gnuPlotStatus = saveGnuplot(gnudump, optimizer);
if (! gnuPlotStatus) {
cerr << "Error while writing gnuplot files" << endl;
}
}
if (outputfilename.size() > 0) {
if (outputfilename == "-") {
cerr << "saving to stdout";
optimizer.save(cout);
} else {
cerr << "saving " << outputfilename << " ... ";
optimizer.save(outputfilename.c_str());
}
cerr << "done." << endl;
}
// destroy all the singletons
//Factory::destroy();
//OptimizationAlgorithmFactory::destroy();
//HyperGraphActionLibrary::destroy();
return 0;
}
int main (int argc , char ** argv){
int maxIterations;
bool verbose;
bool robustKernel;
double lambdaInit;
CommandArgs arg;
bool fixSensor;
bool fixPlanes;
bool fixFirstPose;
bool fixTrajectory;
bool planarMotion;
bool listSolvers;
string strSolver;
cerr << "graph" << endl;
arg.param("i", maxIterations, 5, "perform n iterations");
arg.param("v", verbose, false, "verbose output of the optimization process");
arg.param("solver", strSolver, "lm_var", "select one specific solver");
arg.param("lambdaInit", lambdaInit, 0, "user specified lambda init for levenberg");
arg.param("robustKernel", robustKernel, false, "use robust error functions");
arg.param("fixSensor", fixSensor, false, "fix the sensor position on the robot");
arg.param("fixTrajectory", fixTrajectory, false, "fix the trajectory");
arg.param("fixFirstPose", fixFirstPose, false, "fix the first robot pose");
arg.param("fixPlanes", fixPlanes, false, "fix the planes (do localization only)");
arg.param("planarMotion", planarMotion, false, "robot moves on a plane");
arg.param("listSolvers", listSolvers, false, "list the solvers");
arg.parseArgs(argc, argv);
SparseOptimizer* g=new SparseOptimizer();
ParameterSE3Offset* odomOffset=new ParameterSE3Offset();
odomOffset->setId(0);
g->addParameter(odomOffset);
OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
OptimizationAlgorithmProperty solverProperty;
OptimizationAlgorithm* solver = solverFactory->construct(strSolver, solverProperty);
g->setAlgorithm(solver);
if (listSolvers){
solverFactory->listSolvers(cerr);
return 0;
}
if (! g->solver()){
cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl;
cerr << "available solvers: " << endl;
solverFactory->listSolvers(cerr);
cerr << "--------------" << endl;
return 0;
}
cerr << "sim" << endl;
Simulator* sim = new Simulator(g);
cerr << "robot" << endl;
Robot* r=new Robot(g);
cerr << "planeSensor" << endl;
Matrix3d R=Matrix3d::Identity();
R <<
0, 0, 1,
-1, 0, 0,
0, -1, 0;
Isometry3d sensorPose=Isometry3d::Identity();
sensorPose.matrix().block<3,3>(0,0) = R;
sensorPose.translation()= Vector3d(.3 , 0.5 , 1.2);
PlaneSensor* ps = new PlaneSensor(r, 0, sensorPose);
ps->_nplane << 0.03, 0.03, 0.005;
r->_sensors.push_back(ps);
sim->_robots.push_back(r);
cerr << "p1" << endl;
Plane3D plane;
PlaneItem* pi =new PlaneItem(g,1);
plane.fromVector(Eigen::Vector4d(0.,0.,1.,5.));
static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane);
pi->vertex()->setFixed(fixPlanes);
sim->_world.insert(pi);
plane.fromVector(Eigen::Vector4d(1.,0.,0.,5.));
pi =new PlaneItem(g,2);
static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane);
pi->vertex()->setFixed(fixPlanes);
sim->_world.insert(pi);
cerr << "p2" << endl;
pi =new PlaneItem(g,3);
plane.fromVector(Eigen::Vector4d(0.,1.,0.,5.));
static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane);
pi->vertex()->setFixed(fixPlanes);
sim->_world.insert(pi);
Quaterniond q, iq;
if (planarMotion) {
r->_planarMotion = true;
r->_nmovecov << 0.01, 0.0025, 1e-9, 0.001, 0.001, 0.025;
q = Quaterniond(AngleAxisd(0.2, Vector3d::UnitZ()).toRotationMatrix());
iq = Quaterniond(AngleAxisd(-0.2, Vector3d::UnitZ()).toRotationMatrix());
} else {
r->_planarMotion = false;
//r->_nmovecov << 0.1, 0.005, 1e-9, 0.05, 0.001, 0.001;
r->_nmovecov << 0.1, 0.005, 1e-9, 0.001, 0.001, 0.05;
q = Quaterniond((AngleAxisd(M_PI/10, Vector3d::UnitZ()) * AngleAxisd(0.1, Vector3d::UnitY())).toRotationMatrix());
iq = Quaterniond((AngleAxisd(-M_PI/10, Vector3d::UnitZ()) * AngleAxisd(0.1, Vector3d::UnitY())).toRotationMatrix());
}
Isometry3d delta=Isometry3d::Identity();
sim->_lastVertexId=4;
Isometry3d startPose=Isometry3d::Identity();
startPose.matrix().block<3,3>(0,0) = AngleAxisd(-0.75*M_PI, Vector3d::UnitZ()).toRotationMatrix();
sim->move(0,startPose);
int k =20;
int l = 2;
double delta_t = 0.2;
for (int j=0; j<l; j++) {
Vector3d tr(1.,0.,0.);
delta.matrix().block<3,3>(0,0) = q.toRotationMatrix();
if (j==(l-1)){
delta.matrix().block<3,3>(0,0) = Matrix3d::Identity();
}
delta.translation()=tr*(delta_t*j);
Isometry3d iDelta = delta.inverse();
for (int a=0; a<2; a++){
for (int i=0; i<k; i++){
cerr << "m";
if (a==0)
sim->relativeMove(0,delta);
else
sim->relativeMove(0,iDelta);
cerr << "s";
sim->sense(0);
}
}
}
for (int j=0; j<l; j++) {
Vector3d tr(1.,0.,0.);
delta.matrix().block<3,3>(0,0) = iq.toRotationMatrix();
if (j==l-1){
delta.matrix().block<3,3>(0,0) = Matrix3d::Identity();
}
delta.translation()=tr*(delta_t*j);
Isometry3d iDelta = delta.inverse();
for (int a=0; a<2; a++){
for (int i=0; i<k; i++){
cerr << "m";
if (a==0)
sim->relativeMove(0,delta);
else
sim->relativeMove(0,iDelta);
cerr << "s";
sim->sense(0);
}
}
}
ofstream os("test_gt.g2o");
g->save(os);
if (fixSensor) {
ps->_offsetVertex->setFixed(true);
} else {
Vector6d noffcov;
noffcov << 0.1,0.1,0.1,0.5, 0.5, 0.5;
ps->_offsetVertex->setEstimate(ps->_offsetVertex->estimate() * sample_noise_from_se3(noffcov));
ps->_offsetVertex->setFixed(false);
}
if (fixFirstPose){
OptimizableGraph::Vertex* gauge = g->vertex(4);
if (gauge)
gauge->setFixed(true);
} // else {
// // multiply all vertices of the robot by this standard quantity
// Quaterniond q(AngleAxisd(1, Vector3d::UnitZ()).toRotationMatrix());
// Vector3d tr(1,0,0);
// Isometry3d delta;
// delta.matrix().block<3,3>(0,0)=q.toRotationMatrix();
// delta.translation()=tr;
// for (size_t i=0; i< g->vertices().size(); i++){
// VertexSE3 *v = dynamic_cast<VertexSE3 *>(g->vertex(i));
// if (v && v->id()>0){
// v->setEstimate (v->estimate()*delta);
// }
// }
// }
ofstream osp("test_preopt.g2o");
g->save(osp);
//g->setMethod(SparseOptimizer::LevenbergMarquardt);
g->initializeOptimization();
g->setVerbose(verbose);
g->optimize(maxIterations);
if (! fixSensor ){
SparseBlockMatrix<MatrixXd> spinv;
std::pair<int, int> indexParams;
indexParams.first = ps->_offsetVertex->hessianIndex();
indexParams.second = ps->_offsetVertex->hessianIndex();
std::vector<std::pair <int, int> > blockIndices;
blockIndices.push_back(indexParams);
if (!g->computeMarginals(spinv, blockIndices)){
cerr << "error in computing the covariance" << endl;
} else {
MatrixXd m = *spinv.block(ps->_offsetVertex->hessianIndex(), ps->_offsetVertex->hessianIndex());
cerr << "Param covariance" << endl;
cerr << m << endl;
cerr << "OffsetVertex: " << endl;
ps->_offsetVertex->write(cerr);
cerr << endl;
cerr << "rotationDeterminant: " << m.block<3,3>(0,0).determinant() << endl;
cerr << "translationDeterminant: " << m.block<3,3>(3,3).determinant() << endl;
cerr << endl;
}
}
ofstream os1("test_postOpt.g2o");
g->save(os1);
}
