void _backup(const char *dst, const char *src, bool writeFile=true)
{
CStringTuple *item = lookup.find(dst);
assertex(!item);
item = new CStringTuple(dst, src);
if (async)
{
todo.enqueue(item);
lookup.add(* item);
if (writeFile)
write();
}
else
{
{
CriticalBlock b(crit);
currentItem.setown(item);
}
doBackup(currentItem, false);
{
CriticalBlock b(crit);
currentItem.clear();
}
}
}
void DeterministicSLAM::handleScan(double timestamp, const PointScan &pscan)
{
doBackup(timestamp);
if(doScanMatching(timestamp))
{
SegmentScan sm(pscan, SegmentScan::SplitAndMergeInterpolation);
SemiDeterministicRetriever dr(segments, sm, currentPose *
OdometryCovarianceModel::addCovariance(deltaOdometry));
matcher->setRetriever(dr);
matcher->run();
Eigen::Vector3d z = matcher->measure();
currentPose = z;
deltaOdometry = Rototranslation();
if(!almostEqual(ins.getPose().theta(), 0, 0.02) ||
!almostEqual(ins.getPose().phi(), 0, 0.02)) {
return;
}
if(takeAMeasure(dr, matcher->associations()))
{
SegmentScan rotoscan = Rototranslation(z) * sm;
mergeSegments(rotoscan);
mergeOtherSegments(rotoscan);
if(outdoor){
qDebug("SONO IN OUTDOOR!!!");
frontiers.clear();
}
else{
mergeFrontiers(rotoscan);
}
poses.append(TimedPose(timestamp, currentPose));
lastMeasurePose = lastPose;
lastMeasureTimestamp = timestamp;
if(outdoor){
poseVisibilities.clear();
}
while(poseVisibilities.size()<poses.size()){
poseVisibilities.append(new VisibilityPolygon(rotoscan.toPolygon()));
}
}
if(timestamp - lastThinningTimestamp >= SLAM_MAP_THINNING_INTERVAL) {
mapThinning();
lastThinningTimestamp = timestamp;
}
Pose p = Rototranslation(initialPose) * currentPose;
ldbg<<"SLAM(Scan): Pose is "<<p <<" with timestamp "<<lastPose.timestamp()<<" at "<<QTime::currentTime().toString("hh:mm:ss:zzz")<<endl;
emit newRobotPose(TimedPose(lastPose.timestamp(), p));
}
}
void ICLSLAM::handleScan(double timestamp, const PointScan &pscan)
{
doBackup(timestamp);
if(doICL(timestamp)) {
SegmentScan sm(pscan);
bool skip = false;
if(outdoor) {
Rototranslation rt(currentPose);
freeContent();
fforeach(const LineSegment &s, sm.getSegments()) {
segments.append(new LineSegment(rt * s));
}
fforeach(const Frontier &f, sm.getFrontiers()) {
if(f.length() > 0.2) frontiers.append(new Frontier(rt * f));
}
if(takeAMeasure(timestamp)) {
poses.append(TimedPose(timestamp, currentPose));
lastMeasurePose = lastPose;
lastMeasureTimestamp = timestamp;
}
return;
}
ICLImpl icl(sm, segments, currentPose);
icl.run();
Eigen::Vector3d z = icl.measure();
#ifndef SLAM_SKIP_DEBUG
ldbg << "ins: " << ins.getPose() << endl;
ldbg << "currentPose: " << currentPose << endl;
ldbg << "z: " << z << endl;
#endif
Eigen::Vector3d diff = currentPose.vectorForm() - z;
if(std::abs(wrap(diff[2])) >= M_PI / 12 || SQUARE(diff[0]) + SQUARE(diff[1]) >= 3) {
for(int i = 0; i < 10; i++) {
Rototranslation perturbation(
Random::normal(0, 1 / 3.),
Random::normal(0, 1 / 3.),
Random::normal(0, SQUARE(M_PI / 6) / 3.));
ICLImpl icl(sm, segments, perturbation * currentPose);
icl.run();
const Eigen::Vector3d &z1 = icl.measure();
if(poseDistance(z1, currentPose) < poseDistance(z, currentPose)) {
#ifndef SLAM_SKIP_DEBUG
ldbg << "Cambiato" << endl;
#endif
z = z1;
}
}
}
diff = currentPose.vectorForm() - z;
if(std::abs(wrap(diff[2])) >= M_PI / 12 || SQUARE(diff[0]) + SQUARE(diff[1]) >= 3) {
z = currentPose;
skipCount++;
skip = true;
}
if(skip && skipCount <= MAX_SKIP_COUNT) {
return;
} else {
skipCount = 0;
}
#ifndef SLAM_SKIP_DEBUG
ldbg << "ins: " << ins.getPose() << endl;
ldbg << "currentPose: " << currentPose << endl;
ldbg << "z: " << z << endl;
#endif
Pose guess = currentPose;
currentPose = z;
if(!almostEqual(ins.getPose().theta(), 0, 0.02) ||
!almostEqual(ins.getPose().phi(), 0, 0.02)) {
return;
}
SegmentScan rotoscan = Rototranslation(z) * sm;
#ifndef SLAM_SKIP_DEBUG
ldbg << endl << "counter=" << counter++;
ldbg << endl << "guess=" << guess << endl;
#endif
//printMap(rotoscan.getSegments(), "scan");
//printMap(segments, "walls");
//printMap(frontiers, "frontiers");
//ldbg << "plot(xwalls, ywalls, 'b', xfrontiers, yfrontiers, 'g', xscan, yscan, 'r');" << endl;
//printMapCPP(sm.getSegments(), "scan");
//printMapCPP(segments, "segments");
//ldbg << "Graphics[" << rotoscan.getSegments() << ",Dashed," << rotoscan.getFrontiers() << "]" << endl;
//ldbg << "Graphics[{" << segments << ",Dashed," << frontiers << ",Opacity[0.1],"
// << rotoscan.toPolygon() << "}]" << endl;
mergeSegments(rotoscan);
if(takeAMeasure(timestamp)) {
mergeFrontiers(rotoscan);
poses.append(TimedPose(timestamp, currentPose));
lastMeasurePose = lastPose;
lastMeasureTimestamp = timestamp;
}
if(timestamp - lastThinningTimestamp >= SLAM_MAP_THINNING_INTERVAL) {
#ifndef SLAM_SKIP_DEBUG
ldbg << "Map Thinning" << endl;
#endif
mapThinning();
lastThinningTimestamp = timestamp;
}
#ifndef SLAM_SKIP_DEBUG
//ldbg << endl << "ListPlot[" << pscan << "]" << endl;
ldbg << endl << "Graphics[" << sm.getSegments() << "]";
ldbg << endl << "Graphics[{" << segments << ",Dashed," << frontiers << ",Opacity[0.1],"
<< rotoscan.toPolygon() << "}]" << endl;
#endif
/*printMap(rotoscan.getSegments(), "scan");
printMap(segments, "walls");
printMap(frontiers, "frontiers");
ldbg << "plot(xwalls, ywalls, 'b', xfrontiers, yfrontiers, 'g', xscan, yscan, 'r');" << endl;
*/
//if(counter == 1508)
// exit(0);
Pose p = Rototranslation(initialPose) * currentPose;
emit newRobotPose(TimedPose(lastPose.timestamp(), p));
}
}
