posetracker.cpp

Go to the documentation of this file.

/***************************************************************
 * ROS port of ROBULAR implementation
 *
 * Original header: 
 * *
 * This source code is part of ROBULAR.
 *
 * ROBULAR Copyright (c) 2009 Joerg Mueller, Boris Lau, Kai
 * Wurm. Autonomous Intelligent Systems, Department of Computer
 * Science, University of Freiburg, Germany.
 **************************************************************/

#include "posetracker.h"
#include <fstream>
#include <Eigen/SVD>
#include <eigen_conversions/eigen_msg.h>

  PoseTracker::PoseTracker() :
      initialized(false)
  {
    debug = false;
  }

  void PoseTracker::setDebug(bool d)
  {
    debug = d;
  }

  void PoseTracker::init(const std::string &propFile, double scale, tf::TransformBroadcaster* bc, double _sqrErrThreshold, bool _useRansac, unsigned int _numRansacSamples)
  {

    broadcaster = bc;

    sqrErrThreshold = _sqrErrThreshold;
    useRansac = _useRansac;
    numRansacSamples = _numRansacSamples;

    std::cerr << "=== Initialized PoseTracker ===\n";
    std::cerr << "scale " << scale << " m (only used for parsing propFile)\n";
    std::cerr << "propFile " << propFile << "\n";
    std::cerr << "bodyName " << bodyName << "\n";
    std::cerr << "sqrErrThreshold " << sqrErrThreshold << " m^2\n";
    std::cerr << "useRansac " << useRansac << "\n";
    std::cerr << "numRansacSamples " << numRansacSamples << "\n";
    std::cerr << "===============================\n";

    parsePropFile(propFile, scale);
    initialized = true;
  }


  void PoseTracker::process(const visualization_msgs::MarkerArray::ConstPtr &msg)
  {
    if (!initialized)
      return;

      calculatePose(msg);
  }

  bool PoseTracker::calculatePose(const visualization_msgs::MarkerArray::ConstPtr &m)
  {

    bool foundBody = false;
    unsigned int bodyIdx;
    for(unsigned int i=0; i<m->markers.size(); i++){
      if(m->markers.at(i).ns == bodyName){
        foundBody = true;
        bodyIdx = i;
        break;
      }

      if(!foundBody){
        std::cerr << "Error: body " << bodyName << " was not detected\n";
        return false;
      }
    }

    // calculate robot pose from markers and configuration
    // collect evaluation data
    markerEval.clear();
    for(unsigned int i=0; i<m->markers[bodyIdx].points.size(); ++i){

      double posx = m->markers[bodyIdx].points.at(i).x;
      double posy = m->markers[bodyIdx].points.at(i).y;
      double posz = m->markers[bodyIdx].points.at(i).z;

      if(isnan(posx) || isnan(posy))
        continue;

      MarkerEval me;
      me.model = bodyMarkerPositions[i];
      me.real(0) = posx;
      me.real(1) = posy;
      me.real(2) = posz;
      me.use = false;
      markerEval.push_back(me);
    }

    if (useRansac) {
      std::vector<MarkerEval> best;
      int bestInliers = 0;

      if (markerEval.size() > 6) {
        // real RANSAC
        for (int i=0; i<numRansacSamples; i++) {
          int useIdx[3];
          useIdx[0] = rand()/(double) RAND_MAX * markerEval.size()-1;
          useIdx[1] = rand()/(double) RAND_MAX * markerEval.size()-2;
          useIdx[2] = rand()/(double) RAND_MAX * markerEval.size()-3;
          if (useIdx[1] >= useIdx[0])
            useIdx[1]++;
          if (useIdx[2] >= std::min(useIdx[0], useIdx[1]))
            useIdx[2]++;
          if (useIdx[2] >= std::max(useIdx[0], useIdx[1]))
            useIdx[2]++;
          std::vector<MarkerEval> sample = markerEval;
          sample[useIdx[0]].use = true;
          sample[useIdx[1]].use = true;
          sample[useIdx[2]].use = true;
          Eigen::Affine3d sp;
          int si = 0;
          getConsensus(sample, sp, si);
          if (si > bestInliers) {
            best = sample;
            bestInliers = si;
          }
        }
      } else {
        // try all combinations for model fitting
        for (unsigned int i=0; i<markerEval.size(); i++) {
          for (unsigned int j=i+1; j<markerEval.size(); j++) {
            for (unsigned int k=j+1; k<markerEval.size(); k++) {
              std::vector<MarkerEval> sample = markerEval;
              sample[i].use = true;
              sample[j].use = true;
              sample[k].use = true;
              Eigen::Affine3d sp;
              int si = 0;
              getConsensus(sample, sp, si);
              if (si > bestInliers) {
                best = sample;
                bestInliers = si;
              }
            }
          }
        }
      }
      if (bestInliers < 3) {
        std::cerr << "too less inliers (" << bestInliers << ") - withdraw\n";
        return false;
      }
      markerEval = best;
    } else {
      for (std::vector<MarkerEval>::iterator it = markerEval.begin();
          it != markerEval.end(); it++) {
        it->use = true;
      }
    }


    Eigen::Affine3d p;
    int numInliers = 0;
    double sqrErrSum = getConsensus(markerEval, p, numInliers);

    if (sqrErrSum > sqrErrThreshold) {
      std::cerr << "Warning: Too high remaining sum of squared position errors: " << sqrErrSum << " - no pose found\n";
      return false;
    }

    if (debug) {
      std::cerr << "Transformation (" << markerEval.size() << ") " << p.translation() <<" " <<p.rotation() << ", num inliers " << numInliers << ", sqrt(squared error sum) " << sqrt(sqrErrSum) << "\n";
    }

    geometry_msgs::Pose geo_pose;
    tf::poseEigenToMsg(p, geo_pose);

    geometry_msgs::TransformStamped transf;
    transf.transform.translation.x = geo_pose.position.x;
    transf.transform.translation.y = geo_pose.position.y;
    transf.transform.translation.z = geo_pose.position.z;

    transf.transform.rotation = geo_pose.orientation;
    transf.header.stamp = m->markers[bodyIdx].header.stamp;
    transf.header.frame_id = m->markers[bodyIdx].header.frame_id;
    transf.header.seq = m->markers[bodyIdx].header.seq;
    transf.child_frame_id = bodyName;


    broadcaster->sendTransform(transf);
    return true;
  }

  double PoseTracker::getConsensus(std::vector<MarkerEval> &points, Eigen::Affine3d &p, int &numInliers)
  {
    int useCount = 0;
    Eigen::Vector3d modelSum, realSum;
    for (std::vector<MarkerEval>::const_iterator it = points.begin();
        it != points.end(); it++) {
      if (it->use) {
        modelSum += it->model;
        realSum += it->real;
        useCount++;
      }
    }

    if (useCount < 3) {
      std::cerr << "Error: not enough markers selected for model fitting (" << useCount << ")\n";
      return -1;
    }

    // get transformation by 1 ICP step
    modelSum /= useCount;
    realSum /= useCount;

    Eigen::Matrix3d W;
    for (std::vector<MarkerEval>::const_iterator it = points.begin();
        it != points.end(); it++) {
      W = W + (it->real-realSum) * (it->model-modelSum).transpose();
    }

    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    Eigen::Vector3d d = svd.singularValues();

    Eigen::Matrix3d S;
    S.setIdentity();

    if (U.determinant() * V.determinant() < 0)
      S(2,2) = -1;
    Eigen::Matrix3d rotM = (U*S)*V.transpose();
    Eigen::Vector3d t = realSum - rotM*modelSum;
    p.setIdentity();
    p.translate(t);
    p.rotate(rotM);

    // calculate inliers and squared error
    double sqrErrSum = 0;
    numInliers = 0;
    for (std::vector<MarkerEval>::iterator it = points.begin();
        it != points.end(); it++) {
//      it->sqrErr = p.transform(it->model).sqrDist(it->real); // less efficient
      Eigen::Vector3d tmp = it->real - rotM*it->model - t;
      it->sqrErr = tmp.dot(tmp);
      if (it->sqrErr < sqrErrThreshold) {
        numInliers++;
        sqrErrSum += it->sqrErr;
        it->use = true;
      }
    }
    return sqrErrSum;
  }


  void PoseTracker::parsePropFile(const std::string &logfile, double scale){
    std::ifstream is(logfile.c_str());
    if (!is.is_open()) {
      std::cerr << "Error while opening prop-file '" << logfile << "'.\n";
      return;
    }

    std::string line;
    getline(is, line);
    if (line.find("[RigidBody]") == std::string::npos) {
      std::cerr << "Object of prop file has to be a rigid body\n";
      return;
    }
    getline(is, line);
    if (line.find("Name=") == std::string::npos) {
      std::cerr << "Expected name in second line\n";
      return;
    }
    bodyName = line.substr(5);
    if (bodyName[bodyName.length()-1] == '\r')
      bodyName = bodyName.substr(0, bodyName.length()-1);
    while (!is.eof() && line.find("NumberOfMarkers=") == std::string::npos) {
      getline(is, line);
    }
    if (is.eof()) {
      std::cerr << "Error while parsing prop-file\n";
      return;
    }
    int numMarkers = atoi(line.substr(16).c_str());
    std::cerr << "Reading object '" << bodyName << "' with " << numMarkers << " markers\n";
    for (int i=0; i<numMarkers; i++) {
      getline(is, line);
      size_t pos;
      while ((pos = line.find(',')) != std::string::npos)
        line[pos] = ' ';

      Eigen::Vector3d markerpos;
      std::istringstream ss(line);
      ss >> markerpos(0);
      ss >> markerpos(1);
      ss >> markerpos(2);
      markerpos *= scale;
      double dummy;
      ss >> dummy;
      std::string markerName;
      ss >> markerName;
      if (ss.fail()) {
        std::cerr << "Failed to read marker " << i << "\n";
        return;
      }
      bodyMarkerPositions.push_back(markerpos);
      bodyMarkerNames.push_back(markerName);
    }
  }