Ejemplo n.º 1
0
  bool extractAbsolutePrior(Eigen::Isometry3f& priorMean, Matrix6f& priorInfo, 
			    const DrawableFrame* current){
    VertexSE3* currentVertex =current->_vertex;
    ImuData* imuData = 0;
    OptimizableGraph::Data* d = currentVertex->userData();
    while(d) {
      ImuData* imuData_ = dynamic_cast<ImuData*>(d);
      if (imuData_){
	imuData = imuData_;
      }
      d=d->next();
    }
	
    if (imuData){
      Eigen::Matrix3d R=imuData->getOrientation().matrix();
      Eigen::Matrix3d Omega = imuData->getOrientationCovariance().inverse();
      priorMean.setIdentity();
      priorInfo.setZero();
      for (int c = 0; c<3; c++)
	for (int r = 0; r<3; r++)
	  priorMean.linear()(r,c)=R(r,c);
      
      for (int c = 0; c<3; c++)
	for (int r = 0; r<3; r++)
	  priorInfo(r+3,c+3)=Omega(r,c);
      return true;
    }
    return false;
  }
Ejemplo n.º 2
0
int main(int argc, char **argv) {
  /************************************************************************
   *                          Input handling                              *
   ************************************************************************/
  float rows, cols, gain, square_size;
  float resolution, max_range, usable_range, angle, threshold;
  string g2oFilename, mapFilename;
  g2o::CommandArgs arg;
  arg.param("resolution", resolution, 0.05f, "resolution of the map (how much is in meters a pixel)");
  arg.param("threshold", threshold, -1.0f, "threshold to apply to the frequency map (values under the threshold are discarded)");
  arg.param("rows", rows, 0, "impose the resulting map to have this number of rows");
  arg.param("cols", cols, 0, "impose the resulting map to have this number of columns");
  arg.param("max_range", max_range, -1.0f, "max laser range to consider for map building");
  arg.param("usable_range", usable_range, -1.0f, "usable laser range for map building");
  arg.param("gain", gain, 1, "gain to impose to the pixels of the map");
  arg.param("square_size", square_size, 1, "square size of the region where increment the hits");
  arg.param("angle", angle, 0, "rotate the map of x degrees");
  arg.paramLeftOver("input_graph.g2o", g2oFilename, "", "input g2o graph to use to build the map", false);
  arg.paramLeftOver("output_map", mapFilename, "", "output filename where to save the map (without extension)", false);  
  arg.parseArgs(argc, argv);

  angle = angle*M_PI/180.0;

  /************************************************************************
   *                          Loading Graph                               *
   ************************************************************************/
  // Load graph
  typedef BlockSolver< BlockSolverTraits<-1, -1> >  SlamBlockSolver;
  typedef LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
  SlamLinearSolver *linearSolver = new SlamLinearSolver();
  linearSolver->setBlockOrdering(false);
  SlamBlockSolver *blockSolver = new SlamBlockSolver(linearSolver);
  OptimizationAlgorithmGaussNewton *solverGauss = new OptimizationAlgorithmGaussNewton(blockSolver);
  SparseOptimizer *graph = new SparseOptimizer();
  graph->setAlgorithm(solverGauss);    
  graph->load(g2oFilename.c_str());
  
  // Sort verteces
  vector<int> vertexIds(graph->vertices().size());
  int k = 0;
  for(OptimizableGraph::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) {
    vertexIds[k++] = (it->first);
  }  
  sort(vertexIds.begin(), vertexIds.end());
  
  /************************************************************************
   *                          Compute map size                            *
   ************************************************************************/
  // Check the entire graph to find map bounding box
  Eigen::Matrix2d boundingBox = Eigen::Matrix2d::Zero();
  std::vector<RobotLaser*> robotLasers;
  std::vector<SE2> robotPoses;
  double xmin=std::numeric_limits<double>::max();
  double xmax=std::numeric_limits<double>::min();
  double ymin=std::numeric_limits<double>::max();
  double ymax=std::numeric_limits<double>::min();

  SE2 baseTransform(0,0,angle);

  for(size_t i = 0; i < vertexIds.size(); ++i) {
    OptimizableGraph::Vertex *_v = graph->vertex(vertexIds[i]);
    VertexSE2 *v = dynamic_cast<VertexSE2*>(_v);
    if(!v) { continue; }
    v->setEstimate(baseTransform*v->estimate());
    OptimizableGraph::Data *d = v->userData();

    while(d) {
      RobotLaser *robotLaser = dynamic_cast<RobotLaser*>(d);
      if(!robotLaser) {
	d = d->next();
	continue;
      }      
      robotLasers.push_back(robotLaser);
      robotPoses.push_back(v->estimate());
      double x = v->estimate().translation().x();
      double y = v->estimate().translation().y();
      
      xmax = xmax > x+usable_range ? xmax : x+usable_range;
      ymax = ymax > y+usable_range ? ymax : y+usable_range;
      xmin = xmin < x-usable_range ? xmin : x-usable_range;
      ymin = ymin < y-usable_range ? ymin : y-usable_range;
 
      d = d->next();
    }
  }

  boundingBox(0,0)=xmin;
  boundingBox(0,1)=xmax;
  boundingBox(1,0)=ymin;
  boundingBox(1,1)=ymax;

  std::cout << "Found " << robotLasers.size() << " laser scans"<< std::endl;
  std::cout << "Bounding box: " << std::endl << boundingBox << std::endl; 
  if(robotLasers.size() == 0)  {
    std::cout << "No laser scans found ... quitting!" << std::endl;
    return 0;
  }

  /************************************************************************
   *                          Compute the map                             *
   ************************************************************************/
  // Create the map
  Eigen::Vector2i size;
  if(rows != 0 && cols != 0) { size = Eigen::Vector2i(rows, cols); }
  else {
    size = Eigen::Vector2i((boundingBox(0, 1) - boundingBox(0, 0))/ resolution,
			   (boundingBox(1, 1) - boundingBox(1, 0))/ resolution);
    } 
  std::cout << "Map size: " << size.transpose() << std::endl;
  if(size.x() == 0 || size.y() == 0) {
    std::cout << "Zero map size ... quitting!" << std::endl;
    return 0;
  }

  

  //Eigen::Vector2f offset(-size.x() * resolution / 2.0f, -size.y() * resolution / 2.0f);
  Eigen::Vector2f offset(boundingBox(0, 0),boundingBox(1, 0));
  FrequencyMapCell unknownCell;
  FrequencyMap map = FrequencyMap(resolution, offset, size, unknownCell);

  for(size_t i = 0; i < vertexIds.size(); ++i) {
    OptimizableGraph::Vertex *_v = graph->vertex(vertexIds[i]);
    VertexSE2 *v = dynamic_cast<VertexSE2*>(_v);
    if(!v) { continue; }
    OptimizableGraph::Data *d = v->userData();
    SE2 robotPose = v->estimate();
    
    while(d) {
      RobotLaser *robotLaser = dynamic_cast<RobotLaser*>(d);
      if(!robotLaser) {
	d = d->next();
	continue;
      }      
      map.integrateScan(robotLaser, robotPose, max_range, usable_range, gain, square_size);
      d = d->next();
    }
  }

  /************************************************************************
   *                          Save map image                              *
   ************************************************************************/
  cv::Mat mapImage(map.rows(), map.cols(), CV_8UC1);
  mapImage.setTo(cv::Scalar(0));
  for(int c = 0; c < map.cols(); c++) {
    for(int r = 0; r < map.rows(); r++) {
      if(map(r, c).misses() == 0 && map(r, c).hits() == 0) {
	mapImage.at<unsigned char>(r, c) = 127;
      } else {
	float fraction = (float)map(r, c).hits()/(float)(map(r, c).hits()+map(r, c).misses());
	
	if (threshold > 0 && fraction > threshold)
	  mapImage.at<unsigned char>(r, c) = 0;
	else if (threshold > 0 && fraction <= threshold)
	  mapImage.at<unsigned char>(r, c) = 255;
	else {
	  float val = 255*(1-fraction);
	  mapImage.at<unsigned char>(r, c) = (unsigned char)val;
	}

      }
      // else if(map(r, c).hits() > threshold) {
      // 	mapImage.at<unsigned char>(r, c) = 255;
      // }
      // else {
      // 	mapImage.at<unsigned char>(r, c) = 0;
      // }
    }
  }
  cv::imwrite(mapFilename + ".png", mapImage);

  /************************************************************************
   *                          Write yaml file                             *
   ************************************************************************/
  std::ofstream ofs(string(mapFilename + ".yaml").c_str());
  Eigen::Vector3f origin(0.0f, 0.0f, 0.0f);
  ofs << "image: " << mapFilename << ".png" << std::endl
      << "resolution: " << resolution << std::endl
      << "origin: [" << origin.x() << ", " << origin.y() << ", " << origin.z() << "]" << std::endl
      << "negate: 0" << std::endl
      << "occupied_thresh: " << 0.65f << std::endl
      << "free_thresh: " << 0.2f << std::endl;
  return 0;
}
Ejemplo n.º 3
0
  void ViewerState::load(const std::string& filename){
    clear();
    listWidget->clear();
    graph->clear();
    graph->load(filename.c_str());

    vector<int> vertexIds(graph->vertices().size());
    int k=0;
    for (OptimizableGraph::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) {
      vertexIds[k++] = (it->first);
    }

    sort(vertexIds.begin(), vertexIds.end());

    listAssociations.clear();
    size_t maxCount = 20000;
    for(size_t i = 0; i < vertexIds.size() &&  i< maxCount; ++i) {
      OptimizableGraph::Vertex* _v = graph->vertex(vertexIds[i]);
      g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(_v);
      if (! v)
	continue;
      OptimizableGraph::Data* d = v->userData();
      while(d) {
	RGBDData* rgbdData = dynamic_cast<RGBDData*>(d);
	if (!rgbdData){
	  d=d->next();
	  continue;
	}
	// retrieve from the rgb data the index of the parameter
	int paramIndex = rgbdData->paramIndex();
	// retrieve from the graph the parameter given the index  
	g2o::Parameter* _cameraParam = graph->parameter(paramIndex);
	// attempt a cast to a parameter camera  
	ParameterCamera* cameraParam = dynamic_cast<ParameterCamera*>(_cameraParam);
	if (! cameraParam){
	  cerr << "shall thou be damned forever" << endl;
	  return;
	}
	// yayyy we got the parameter
	Eigen::Matrix3f cameraMatrix;
	Eigen::Isometry3f sensorOffset;
	cameraMatrix.setZero();
      
	int cmax = 4;
	int rmax = 3;
	for (int c=0; c<cmax; c++){
	  for (int r=0; r<rmax; r++){
	    sensorOffset.matrix()(r,c)= cameraParam->offset()(r, c);
	    if (c<3)
	      cameraMatrix(r,c) = cameraParam->Kcam()(r, c);
	  }
	}
	char buf[1024];
	sprintf(buf,"%d",v->id());
	QString listItem(buf);
	listAssociations.push_back(rgbdData);
	listWidget->addItem(listItem);
	d=d->next();
      }
    }

  }
Ejemplo n.º 4
0
void Graph2occupancy::computeMap(){

  // Sort verteces
  vector<int> vertexIds(_graph->vertices().size());
  int k = 0;
  for(OptimizableGraph::VertexIDMap::iterator it = _graph->vertices().begin(); it != _graph->vertices().end(); ++it) {
    vertexIds[k++] = (it->first);
  }  
  sort(vertexIds.begin(), vertexIds.end());


  /************************************************************************
   *                          Compute map size                            *
   ************************************************************************/
  // Check the entire graph to find map bounding box
  Matrix2d boundingBox = Matrix2d::Zero();
  std::vector<RobotLaser*> robotLasers;
  std::vector<SE2> robotPoses;
  double xmin=std::numeric_limits<double>::max();
  double xmax=std::numeric_limits<double>::min();
  double ymin=std::numeric_limits<double>::max();
  double ymax=std::numeric_limits<double>::min();

  SE2 baseTransform(0,0,_angle);
  bool initialPoseFound = false;
  SE2 initialPose;
  for(size_t i = 0; i < vertexIds.size(); ++i) {
    OptimizableGraph::Vertex *_v = _graph->vertex(vertexIds[i]);
    VertexSE2 *v = dynamic_cast<VertexSE2*>(_v);
    if(!v) { continue; }
    if (v->fixed() && !initialPoseFound){
      initialPoseFound = true;
      initialPose = baseTransform*v->estimate();
    }
    OptimizableGraph::Data *d = v->userData();

    while(d) {
      RobotLaser *robotLaser = dynamic_cast<RobotLaser*>(d);
      if(!robotLaser) {
	d = d->next();
	continue;
      }      
      robotLasers.push_back(robotLaser);
      SE2 transformed_estimate = baseTransform*v->estimate();
      robotPoses.push_back(transformed_estimate);

      double x = transformed_estimate.translation().x();
      double y = transformed_estimate.translation().y();
      
      xmax = xmax > x+_usableRange ? xmax : x + _usableRange;
      ymax = ymax > y+_usableRange ? ymax : y + _usableRange;
      xmin = xmin < x-_usableRange ? xmin : x - _usableRange;
      ymin = ymin < y-_usableRange ? ymin : y - _usableRange;
 
      d = d->next();
    }
  }

  boundingBox(0,0)=xmin;
  boundingBox(1,0)=ymin;
  boundingBox(0,1)=xmax;
  boundingBox(1,1)=ymax;


  if(robotLasers.size() == 0)  {
    std::cout << "No laser scans found ... quitting!" << std::endl;
    return;
  }

  /************************************************************************
   *                          Compute the map                             *
   ************************************************************************/
  // Create the map
  Vector2i size;
  Vector2f offset;
  if(_rows != 0 && _cols != 0) { 
    size = Vector2i(_rows, _cols); 
  }
  else {
    size = Vector2i((boundingBox(0, 1) - boundingBox(0, 0))/ _resolution, 
		    (boundingBox(1, 1) - boundingBox(1, 0))/ _resolution);
  } 



  if(size.x() == 0 || size.y() == 0) {
    std::cout << "Zero map size ... quitting!" << std::endl;
    return;
  }


  offset = Eigen::Vector2f(boundingBox(0, 0),boundingBox(1, 0));
  FrequencyMapCell unknownCell;
  
  _map = FrequencyMap(_resolution, offset, size, unknownCell);

  for (size_t i = 0; i < robotPoses.size(); ++i) {
    _map.integrateScan(robotLasers[i], robotPoses[i], _maxRange, _usableRange, _infinityFillingRange, _gain, _squareSize);
  }

  /************************************************************************
   *                  Convert frequency map into int[8]                   *
   ************************************************************************/

  *_mapImage = cv::Mat(_map.rows(), _map.cols(), CV_8UC1);
  _mapImage->setTo(cv::Scalar(0));

  for(int c = 0; c < _map.cols(); c++) {
    for(int r = 0; r < _map.rows(); r++) {
      if(_map(r, c).misses() == 0 && _map(r, c).hits() == 0) {
	_mapImage->at<unsigned char>(r, c) = _unknownColor; }
      else {
	float fraction = (float)_map(r, c).hits()/(float)(_map(r, c).hits()+_map(r, c).misses());
	if (_freeThreshold && fraction < _freeThreshold){
	  _mapImage->at<unsigned char>(r, c) = _freeColor; }
	else if (_threshold && fraction > _threshold){
	  _mapImage->at<unsigned char>(r, c) = _occupiedColor; }
	else {
	  _mapImage->at<unsigned char>(r, c) = _unknownColor; }
      }
          
    }
  }

  Eigen::Vector2f origin(0.0f, 0.0f);
  if (initialPoseFound){
    Eigen::Vector2i originMap = _map.world2map(Eigen::Vector2f(initialPose.translation().x(),
							       initialPose.translation().y()));
    origin = Eigen::Vector2f(((-_resolution * originMap.y())+initialPose.translation().y()),
			     -(_resolution * (_mapImage->rows-originMap.x()) +initialPose.translation().x()));
    
  }

  _mapCenter = origin;

}