void MsdpNetwork::translate(DataQueue& msdp)
{
const tbyte* data = (const tbyte*)msdp.getData();
int len = msdp.getSize();
//OUTPUT_BYTES(data, len, len, "MSDP");
while (len > 0)
{
if (len < 3 || data[0] != IAC || data[2] != MSDP)
{
assert(false);
msdp.clear();
return;
}
tbyte cmd = data[1];
if (cmd == SB)
{
const tbyte *p = data;
for (int i=3; i<len; ++i)
{
if (p[i]==SE && p[i-1]==IAC && p[i-2]!=IAC)
{
run_plugins_msdp(p+3, i-4);
len = i+1;
break;
}
}
msdp.truncate(len);
}
else if (cmd == DO)
{
m_state = true;
msdp.truncate(3);
send_varval("CLIENT_NAME", "TORTILLA");
send_varval("CLIENT_VERSION", TW2A(TORTILLA_VERSION));
tortilla::getPluginsManager()->processPluginsMethod("msdpon", 0);
}
else if (cmd == DONT)
{
m_state = false;
msdp.truncate(3);
tortilla::getPluginsManager()->processPluginsMethod("msdpoff", 0);
releaseReports();
}
else
{
assert(false);
msdp.clear();
return;
}
data = (const tbyte*)msdp.getData();
len = msdp.getSize();
}
}
/// Hook function that is called for all incoming messages from the server to the AO client.
Message_t* DataBlockToMessageHook( int _Size, void* _pDataBlock )
{
// Retrieves the number of milliseconds that have elapsed since the system was started, up to 49.7 days.
DWORD time = GetTickCount();
if (time < g_lastTick) {
// Overflow so just reset the g_lastTick
g_lastTick = time;
}
if (time - g_lastTick > 10000) {
// 10 sec since last update
g_lastTick = time;
LoadMessageFilter(HKEY_CURRENT_USER, "Software\\AOMessageHook\\MessageIDs");
}
Header * msg = (Header*)_pDataBlock;
unsigned int msgId = _byteswap_ulong(msg->msgid);
if (g_messageFilter.size() == 0 || g_messageFilter.find(msgId) != g_messageFilter.end())
{
DataItemPtr item(new DataItem(TYPE_INCOMING, _Size, (char*)_pDataBlock));
g_dataQueue.push(item);
SetEvent(g_hEvent);
}
return pOriginalDataBlockToMessage( _Size, _pDataBlock );
}
/// Thread function that dispatches queued message blocks to the AOIA application.
void WorkerThreadMethod(void*)
{
while ((g_hEvent != NULL) && (WaitForSingleObject(g_hEvent,INFINITE) == WAIT_OBJECT_0))
{
if (g_hEvent == NULL) {
break;
}
DWORD tick = GetTickCount();
if (tick < g_lastThreadTick) {
// Overflow
g_lastThreadTick = tick;
}
if ((tick - g_lastThreadTick > 10000) || (g_targetWnd == NULL)) {
// We either don't have a valid window target OR it has been more than 10 sec since we last update the target.
g_targetWnd = FindWindow ( "ItemAssistantWindowClass", NULL ); // TODO: make the class name a list in registry
g_lastThreadTick = GetTickCount();
LOG("FindWindow returned: " << g_targetWnd);
}
while (!g_dataQueue.empty())
{
DataItemPtr item = g_dataQueue.pop();
if (g_targetWnd == NULL) {
// We have data, but no target window, so just delete the message
continue;
}
COPYDATASTRUCT data;
data.dwData = item->type();
data.lpData = item->data();
data.cbData = item->size();
// To avoid blocking the main thread, we should not have a lock on the queue while we process the message.
SendMessage( g_targetWnd, WM_COPYDATA, 0, ( LPARAM ) &data );
LOG("After SendMessage error code is " << GetLastError());
}
}
}
/// Hook function that is called before a message is sent from the AO client to the server.
void OnConnectionSend(void * connection, unsigned char * _msgData, unsigned int len)
{
Header * msg = (Header*)_msgData;
unsigned int msgId = _byteswap_ulong(msg->msgid);
if (msgId == 0x54111123 || msgId == 0x02)//move and keepalive
{
return;
}
LOG("OnConnectionSend( len: " << len << ", msgId: 0x" << std::hex << msgId);
DataItemPtr item(new DataItem(TYPE_OUTGOIING, len, (char*)_msgData));
g_dataQueue.push(item);
SetEvent(g_hEvent);
}
void Session::SendProc()
{
while(_alive == 1)
{
if(_request.type == 0)
{
Sleep(500); // status
}
Sleep(2);
if(_sendStop) continue;
if(_request.type == 4 && !_dataQueue.IsExistFree()) continue;
SendData();
}
}
void* TcpClient::responseFunc(TcpClient* arg)
{
TcpClient* pClient = (TcpClient*)arg;
DataQueue recvDataQueue;
int nDataLen(-1);
while (!pClient->m_bStop)
{
char recvBuf[65535];
int nLen(pClient->getSocket()->recv(recvBuf, sizeof(recvBuf) / sizeof(char), 0));
if (nLen < 0)
{
}
else
{
recvDataQueue.push(recvBuf, nLen);
int nPos(0);
int nPackage(recvDataQueue.getPos());
while (1)
{
if (nPackage <= 0)
break;
CodedInputStream* coded_input = new CodedInputStream((uint8_t*)(recvDataQueue.getBuf() + nPos), recvDataQueue.getPos() - nPos);
autoptr<CodedInputStream> inputguard(coded_input);
int nOldPos = coded_input->CurrentPosition();
uint32_t datalen;
if (!coded_input->ReadVarint32(&datalen))
{
if (nPos > 0)
{
recvDataQueue.pop(nPos);
}
break;
}
int nNewPos = coded_input->CurrentPosition();
if (nNewPos + datalen > nPackage)
{
//一个包没有收整
if (nPos > 0)
{
recvDataQueue.pop(nPos);
}
break;
}
else if (nNewPos + datalen == nPackage)
{
char* data = new char[datalen];
autoptr_arr<char> arrdata(data);
if (coded_input->ReadRaw(data, datalen))
{
OnPackage(data, datalen);
}
recvDataQueue.setPos(0);
break;
}
else
{
char* data = new char[datalen];
autoptr_arr<char> arrdata(data);
if (coded_input->ReadRaw(data, datalen))
{
OnPackage(data, datalen);
}
nPackage -= (datalen+nNewPos-nOldPos);
nPos += (datalen+nNewPos-nOldPos);
}
}
}
}
return NULL;
}
void MsdpNetwork::translateReceived(DataQueue& msdp_data)
{
if (msdp_data.getSize() > 0)
translate(msdp_data);
}
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)
{
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;
}
void Session::ParseData()
{
PacketInfo* response = (PacketInfo*)_recvData;
if(response->type == 1) // ListInfo
{
if(_musicNames != NULL)
{
for(int i = 0; i < _musicCount; i++)
{
if(_musicNames[i] != NULL) delete [] _musicNames[i];
}
}
_musicCount = response->count;
_musicNames = (char**)(new char*[_musicCount]);
for(int i = 0; i < _musicCount; i++) _musicNames[i] = NULL;
_musicOffset = 0;
SetSendBuf(2, response->serialnumber); // ListBlock
}
else if(response->type == 2) // ListBlock
{
if(_musicNames == NULL)
{
SetSendBuf(1, response->serialnumber); // ListInfo
return;
}
if(_musicOffset != _request.offset)
{
SetSendBuf(2, response->serialnumber); // ListBlock
return;
}
int offset = sizeof(PacketInfo);
while(offset < _recvSize && _musicOffset < _musicCount)
{
ListInfo* listInfo = (ListInfo*)(_recvData + offset);
if(listInfo->offset != _musicOffset) break;
offset += sizeof(ListInfo);
if(_recvSize - offset < listInfo->size) break;
_musicNames[_musicOffset] = new char[listInfo->size];
memcpy(_musicNames[_musicOffset], _recvData + offset, listInfo->size);
printf("[%d] %s\n", _musicOffset + 1, _musicNames[_musicOffset]);
_musicOffset++;
offset += listInfo->size;
}
if(_musicOffset < _musicCount)
{
SetSendBuf(2, response->serialnumber); // ListBlock
return;
}
_keyStatus = 1;
SetSendBuf(0, response->serialnumber); // Satatus
}
else if(response->type == 3) // FileInfo
{
_fileSize = response->count;
if(_fileSize == -1)
{
printf("[%s couldn't be read\n", _musicNames[_musicIndex]);
_keyStatus = 1;
SetSendBuf(0, response->serialnumber); // Satatus
return;
}
else if(_fileSize < 1024) printf("\n%s filesize: %d bytes\n", _musicNames[_musicIndex], _fileSize);
else printf("\n%s filesize: %d KB\n", _musicNames[_musicIndex], _fileSize / 1024);
_audio.Start();
_fileOffset = 0;
_keyStatus = 2;
SetSendBuf(4, response->serialnumber); // FileBlock
}
else if(response->type == 4) // FileBlock
{
if(_audio.IsAvailable()) _keyStatus = 2;
_blockSize = _recvSize - sizeof(PacketInfo);
if(_fileOffset != response->offset)
{
SetSendBuf(4, response->serialnumber); // FileBlock
return;
}
_dataQueue.AddQueue(_recvData + sizeof(PacketInfo), _blockSize);
_fileOffset += _blockSize;
if(_fileOffset < _fileSize)
{
SetSendBuf(4, response->serialnumber); // ListBlock
return;
}
_audio.Done();
SetSendBuf(0, response->serialnumber); // Satatus
}
}
int main(int argc, char** argv)
{
string rawFilename;
bool use_viso = false;
bool use_cfs = false;
double init_x, init_y;
cout << endl
<< "\033[32mA demo implementing the method of stereo-odo calibration.\033[0m"
<< endl << "* * * Author: \033[32mDoom @ ZJU.\033[0m"
<< endl << "Usage: sclam_odo_stereo_cal USE_VISO INPUT_FILENAME USE_CLOSEFORM" << endl << endl;
if(argc < 2){
cout << "\033[33m[Warning]:No input directory name, using the default one : /home/doom/CSO/data/params.yaml\033[0m" << endl << endl;
// Read in our param file
CYaml::get().parseParamFile("/home/doom/CSO/data/params.yaml");
// Read in params
use_viso = CYaml::get().general()["use_viso"].as<bool>();
rawFilename = CYaml::get().general()["data_folder"].as<std::string>();
use_cfs = CYaml::get().general()["use_closed_form"].as<bool>();
init_x = CYaml::get().general()["init_x"].as<double>();
init_y = CYaml::get().general()["init_y"].as<double>();
}
else if(argc == 2){
cout << "\033[31mInput params file directory: \033[0m" << argv[1] << endl << endl;
// Read in our param file
CYaml::get().parseParamFile(argv[1]);
// Read in params
use_viso = CYaml::get().general()["use_viso"].as<bool>();
rawFilename = CYaml::get().general()["data_folder"].as<std::string>();
use_cfs = CYaml::get().general()["use_closed_form"].as<bool>();
init_x = CYaml::get().general()["init_x"].as<double>();
init_y = CYaml::get().general()["init_y"].as<double>();
}
else
{
cerr << "\033[31m[FATAL]Bad parameters.\033[0m" << endl;
return 1;
}
// construct a new optimizer
SparseOptimizer optimizer;
initOptimizer(optimizer);
// read the times.txt, to determine how many pics in this directory.
stringstream ss;
ss << rawFilename << "/votimes.txt";
ifstream in(ss.str().c_str());
int Num = 0;
vector<double> timeque;
if(in.fail())
{
cerr << "\033[1;31m[ERROR]Wrong foldername or no times.txt in this directory.\033[0m" << endl;
return 1;
}
else
{
string stemp;
getline(in, stemp, '\n');
while(in.good()){
Num++;
getline(in, stemp, '\n');
}
cout << "There are " << Num << " pics in this direcotory." << endl << endl;
in.close();
in.open(ss.str().c_str());
double temp;
for(int i = 0; i < Num; i++)
{
in >> temp;
timeque.push_back(temp);
}
in.close();
}
// loading odom data
cerr << "\033[31mNow loading odometry data.\033[0m" << endl;
string odomName = rawFilename + "/newodom.txt";
DataQueue odometryQueue;
DataQueue stereovoQueue;
SE2 init_offset;
int numOdom = Gm2dlIO::readRobotOdom(odomName, odometryQueue);
if (numOdom == 0) {
cerr << "No raw information read" << endl;
return 0;
}
cerr << "Done...Read " << numOdom << " odom readings from file" << endl << endl;
// initial first stereo frame pose
SE2 initialStereoPose;
bool first = true;
int numVo = 0;
if(use_viso)
{
cout << "\033[31mUsing libviso...\033[0m" << rawFilename << endl;
RobotOdom* ro = dynamic_cast<RobotOdom*>(odometryQueue.buffer().begin()->second);
// init a libviso2
StereoVo viso(rawFilename, Num, ro, timeque);
// get initial odometry pose in robot frame and stereo vo.
viso.getInitStereoPose(initialStereoPose);
numVo = viso.getMotion(stereovoQueue);
cerr << "Done...There are " << numVo << " vo datas in the queue." << endl << endl;
}
else
{
cerr << "\033[31mLoading pose file...\033[0m" << endl;
ss.str("");
ss << rawFilename << "/CameraTrajectory.txt";
in.open(ss.str().c_str());
int numVo = 0;
Matrix4D posemat;
for(int i = 0; i < Num; i++)
{
for(int j = 0; j < 4; j++)
{
for(int k = 0; k < 4; k++)
in >> posemat(j, k);
}
if(first){
init_offset.setTranslation(Eigen::Vector2d(init_x, init_y));
init_offset.setRotation(Eigen::Rotation2Dd::Identity());
// SE2 x(posemat(2,3),-posemat(0,3),acos(posemat(0,0)));
// RobotOdom* ro = dynamic_cast<RobotOdom*>(odometryQueue.buffer().begin()->second);
initialStereoPose = init_offset;
first = false;
}
// save stereo vo as robotOdom node.
RobotOdom* tempVo = new RobotOdom;
tempVo->setTimestamp(timeque[i]);
SE2 x(posemat(2,3),-posemat(0,3),acos(posemat(0,0)));
tempVo->setOdomPose(init_offset*x);
stereovoQueue.add(tempVo);
numVo++;
}
in.close();
cerr << "Done...There are " << numVo << " vo datas in the queue." << endl << endl;
}
// adding the measurements
vector<MotionInformation, Eigen::aligned_allocator<MotionInformation> > motions;
{
std::map<double, RobotData*>::const_iterator it = stereovoQueue.buffer().begin();
std::map<double, RobotData*>::const_iterator prevIt = it++;
for (; it != stereovoQueue.buffer().end(); ++it) {
MotionInformation mi;
RobotOdom* prevVo = dynamic_cast<RobotOdom*>(prevIt->second);
RobotOdom* curVo = dynamic_cast<RobotOdom*>(it->second);
mi.stereoMotion = prevVo->odomPose().inverse() * curVo->odomPose();
// get the motion of the robot in that time interval
RobotOdom* prevOdom = dynamic_cast<RobotOdom*>(odometryQueue.findClosestData(prevVo->timestamp()));
RobotOdom* curOdom = dynamic_cast<RobotOdom*>(odometryQueue.findClosestData(curVo->timestamp()));
mi.odomMotion = prevOdom->odomPose().inverse() * curOdom->odomPose();
mi.timeInterval = prevOdom->timestamp() - curOdom->timestamp();
prevIt = it;
motions.push_back(mi);
}
}
// Construct the graph.
cerr << "\033[31mConstruct the graph...\033[0m" << endl;
// Vertex offset
addVertexSE2(optimizer, initialStereoPose, 0);
for(int i = 0; i < motions.size(); i++)
{
const SE2& odomMotion = motions[i].odomMotion;
const SE2& stereoMotion = motions[i].stereoMotion;
// add the edge
// stereo vo and odom measurement.
OdomAndStereoMotion meas;
meas.StereoMotion = stereoMotion;
meas.odomMotion = odomMotion;
addEdgeCalib(optimizer, 0, meas, Eigen::Matrix3d::Identity());
}
cerr << "Done..." << endl << endl;
// if you want check the component of your graph, uncomment the CHECK_GRAPH
#ifdef CHECK_GRAPH
for(auto it:optimizer.edges())
{
VertexSE2* v = static_cast<VertexSE2*>(it->vertex(0));
cout << v->id() << endl;
}
#endif
// optimize.
optimize(optimizer, 10);
Eigen::Vector3d result = getEstimation(optimizer);
cerr << "\033[1;32mCalibrated stereo offset (x, y, theta):\033[0m" << result[0] << " " << result [1] << " " << result[2] << endl << endl;
// If you want see how's its closed form solution, uncomment the CLOSED_FORM
if(use_cfs){
cerr << "\033[31mPerforming the closed form solution...\033[0m" << endl;
SE2 closedFormStereo;
ClosedFormCalibration::calibrate(motions, closedFormStereo);
cerr << "\033[1;32mDone... closed form Calibrated stereo offset (x, y, theta):\033[0m" << closedFormStereo.toVector().transpose() << endl;
}
return 0;
}
