コード例 #1
0
ファイル: Sprite.cpp プロジェクト: danomatika/Visual
//--------------------------------------------------------------
void Sprite::setDrawFromCenter(bool yesno) {
	bDrawFromCenter = yesno;
	for(unsigned int i = 0; i < frames.size(); ++i) {
		DrawableFrame* f = frames.at(i);
		f->setDrawFromCenter(bDrawFromCenter);
	}
}
コード例 #2
0
ファイル: Sprite.cpp プロジェクト: danomatika/Visual
//--------------------------------------------------------------
void Sprite::draw() {
	if(frames.empty()) {
		return;
	}

	// animate frames?
	if(bAnimate) {
	
		// go to next frame if time has elapsed
		if(ofGetElapsedTimeMillis() - timestamp > frames.at(currentFrame)->getFrameTime()) {
			if(bForward) {
				nextFrame();
			}
			else {
				prevFrame();
			}
			timestamp = ofGetElapsedTimeMillis();
		}
	}

	// draw frame(s)?
	if(bVisible) {
		if(bDrawAllLayers) {
			for(unsigned int i = 0; i < frames.size(); ++i) {
				DrawableFrame* f = frames.at(i);
				f->draw(pos.x, pos.y);
			}
		}
		else if(currentFrame >= 0 && currentFrame < (int) frames.size()) {
			DrawableFrame* f = frames.at(currentFrame);
			f->draw(pos.x, pos.y);
		}
	}
}
コード例 #3
0
ファイル: Sprite.cpp プロジェクト: danomatika/Visual
// PROTECTED
//--------------------------------------------------------------
void Sprite::resizeIfNecessary() {
	if(width != 0 && height != 0) {
		for(unsigned int i = 0; i < frames.size(); ++i) {
			DrawableFrame* f = frames.at(i);
			f->setSize(width, height);
		}
	}
}
コード例 #4
0
ファイル: Sprite.cpp プロジェクト: danomatika/Visual
//--------------------------------------------------------------
void Sprite::setSize(unsigned int w, unsigned int h) {
	width = w;
	height = h;
	for(unsigned int i = 0; i < frames.size(); ++i) {
		DrawableFrame* f = frames.at(i);
		f->setSize(w, h);
	}
}
コード例 #5
0
ファイル: viewer_state.cpp プロジェクト: 9578577/g2o_frontend
 void ViewerState::initialGuessSelected(){
     cerr << "initial guess" << endl;
     DrawableFrame* current = drawableFrameVector.back();
     DrawableFrame* reference = drawableFrameVector[drawableFrameVector.size()-2];
     current->setTransformation(reference->transformation());
     newCloudAdded = true;
     wasInitialGuess = true;
     *initialGuessViewer = 0;
 }
コード例 #6
0
ファイル: viewer_state.cpp プロジェクト: 9578577/g2o_frontend
  void ViewerState::polishOldThings(){
    if(drawableFrameVector.size() > 40) {
      pwnGMW->viewer_3d->drawableList().erase(pwnGMW->viewer_3d->drawableList().begin());
      DrawableFrame* firstDrawableFrame = drawableFrameVector.front();
      if (0 && firstDrawableFrame->frame()){
	delete firstDrawableFrame->frame();
      }
      delete firstDrawableFrame;
      drawableFrameVector.erase(drawableFrameVector.begin());
    }
  }
コード例 #7
0
ファイル: viewer_state.cpp プロジェクト: 9578577/g2o_frontend
  void ViewerState::clearLastSelected(){
    if(drawableFrameVector.size() > 0) {
      DrawableFrame* lastDrawableFrame = drawableFrameVector.back();
      
      pwnGMW->viewer_3d->popBack();
      //if (lastDrawableFrame->frame()){
      //	delete lastDrawableFrame->frame();
      //}
      if (lastDrawableFrame){
	delete lastDrawableFrame;
      }
      drawableFrameVector.pop_back();
    }
    if(drawableFrameVector.size() > 0) { 
      DrawableFrame* lastDrawableFrame = drawableFrameVector.back();
      globalT = lastDrawableFrame->transformation();;
    } 
  }
コード例 #8
0
ファイル: viewer_state.cpp プロジェクト: 9578577/g2o_frontend
  void ViewerState::addCloudSelected(){
    if(listItem) {
      cerr << "adding a frame" << endl;
      int index = pwnGMW->listWidget->row(listItem);
      cerr << "index: " << endl;
      std::string fname = listAssociations[index]->baseFilename()+"_depth.pgm";
      DepthImage depthImage;
      if (!depthImage.load(fname.c_str(), true)){
      	cerr << " skipping " << fname << endl;
      	newCloudAdded = false;
      	*addCloud = 0;
      	return;
      }
      DepthImage scaledDepthImage;
      DepthImage::scale(scaledDepthImage, depthImage, reduction);
      imageRows = scaledDepthImage.rows();
      imageCols = scaledDepthImage.cols();
      correspondenceFinder->setSize(imageRows, imageCols);
      Frame * frame=new Frame();
      converter->compute(*frame, scaledDepthImage, sensorOffset);
      OptimizableGraph::DataContainer* dc =listAssociations[index]->dataContainer();
      cerr << "datacontainer: " << dc << endl;
      VertexSE3* v = dynamic_cast<VertexSE3*>(dc);
      cerr << "vertex of the frame: " << v->id() << endl;

      DrawableFrame* drawableFrame = new DrawableFrame(globalT, drawableFrameParameters, frame);
      drawableFrame->_vertex = v;
      
      Eigen::Isometry3f priorMean;
      Matrix6f priorInfo;
      bool hasImu =extractAbsolutePrior(priorMean, priorInfo, drawableFrame);
      if (hasImu && drawableFrameVector.size()==0){
        	globalT.linear() = priorMean.linear();
        	cerr << "!!!! i found an imu for the first vertex, me happy" << endl;
        	drawableFrame->setTransformation(globalT);
      }
      drawableFrameVector.push_back(drawableFrame);
      pwnGMW->viewer_3d->addDrawable(drawableFrame);
    }
    newCloudAdded = true;
    *addCloud = 0;
    _meHasNewFrame = true;
  }
コード例 #9
0
ファイル: Sprite.cpp プロジェクト: danomatika/Visual
//--------------------------------------------------------------
void Sprite::setHeight(unsigned int h) {
	for(unsigned int i = 0; i < frames.size(); ++i) {
		DrawableFrame* f = frames.at(i);
		f->setHeight(h);
	}
}
コード例 #10
0
ファイル: Sprite.cpp プロジェクト: danomatika/Visual
//--------------------------------------------------------------
void Sprite::setWidth(unsigned int w) {
	for(unsigned int i = 0; i < frames.size(); ++i) {
		DrawableFrame* f = frames.at(i);
		f->setWidth(w);
	}
}
コード例 #11
0
ファイル: viewer_state.cpp プロジェクト: 9578577/g2o_frontend
  void ViewerState::optimizeSelected(){
      DrawableFrame* current = drawableFrameVector.back();
      DrawableFrame* reference = drawableFrameVector[drawableFrameVector.size()-2];
      cerr << "optimizing" << endl;
      cerr << "current=" << current->frame() << endl;
      cerr << "reference= " << reference->frame() << endl;


      

      // cerr computing initial guess based on the frame positions, just for convenience
      Eigen::Isometry3d delta = reference->_vertex->estimate().inverse()*current->_vertex->estimate();
      for(int c=0; c<4; c++)
	for(int r=0; r<3; r++)
	  initialGuess.matrix()(r,c) = delta.matrix()(r,c);


      Eigen::Isometry3f odometryMean;
      Matrix6f odometryInfo;
      bool hasOdometry = extractRelativePrior(odometryMean, odometryInfo, reference, current);
      if (hasOdometry)
	initialGuess=odometryMean;

      Eigen::Isometry3f imuMean;
      Matrix6f imuInfo;
      bool hasImu = extractAbsolutePrior(imuMean, imuInfo, current);

      initialGuess.matrix().row(3) << 0,0,0,1;
      
      if(!wasInitialGuess) {
	aligner->clearPriors();
	aligner->setOuterIterations(al_outerIterations);
	aligner->setReferenceFrame(reference->frame());
	aligner->setCurrentFrame(current->frame());
	aligner->setInitialGuess(initialGuess);
	aligner->setSensorOffset(sensorOffset);
	if (hasOdometry)
	  aligner->addRelativePrior(odometryMean, odometryInfo);
	if (hasImu)
	  aligner->addAbsolutePrior(reference->transformation(), imuMean, imuInfo);
	aligner->align();
      }

      Eigen::Isometry3f localTransformation =aligner->T();
      if (aligner->inliers()<1000 || aligner->error()/aligner->inliers()>10){
	cerr << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
	cerr << "aligner: monster failure: inliers = " << aligner->inliers() << endl;
	cerr << "aligner: monster failure: error/inliers = " << aligner->error()/aligner->inliers() << endl;
	cerr  << "Local transformation: " << t2v(aligner->T()).transpose() << endl;
      	localTransformation = initialGuess;
	sleep(1);
      }
      cout << "Local transformation: " << t2v(aligner->T()).transpose() << endl;
      
      globalT = reference->transformation()*aligner->T();
      
      // Update cloud drawing position.
      current->setTransformation(globalT);
      current->setLocalTransformation(aligner->T());

      // Show zBuffers.
      refScn->clear();
      currScn->clear();
      QImage refQImage;
      QImage currQImage;
      DepthImageView div;
      div.computeColorMap(300, 2000, 128);
      div.convertToQImage(refQImage, aligner->correspondenceFinder()->referenceDepthImage());
      div.convertToQImage(currQImage, aligner->correspondenceFinder()->currentDepthImage());
      refScn->addPixmap((QPixmap::fromImage(refQImage)).scaled(QSize((int)refQImage.width()/(ng_scale*3), (int)(refQImage.height()/(ng_scale*3)))));
      currScn->addPixmap((QPixmap::fromImage(currQImage)).scaled(QSize((int)currQImage.width()/(ng_scale*3), (int)(currQImage.height()/(ng_scale*3)))));
      pwnGMW->graphicsView1_2d->show();
      pwnGMW->graphicsView2_2d->show();
      
      wasInitialGuess = false;
      newCloudAdded = false;
      *initialGuessViewer = 0;
      *optimizeViewer = 0;
  }
コード例 #12
0
int main (int argc, char** argv) {
    // Handle input
    float pointSize;
    float pointStep;
    float alpha;
    int applyTransform;
    int step;
    string logFilename;
    string configFilename;
    float di_scaleFactor;
    float scale;

    g2o::CommandArgs arg;
    arg.param("vz_pointSize", pointSize, 1.0f, "Size of the points where are visualized");
    arg.param("vz_transform", applyTransform, 1, "Choose if you want to apply the absolute transform of the point clouds");
    arg.param("vz_step", step, 1, "Visualize a point cloud each vz_step point clouds");
    arg.param("vz_alpha", alpha, 1.0f, "Alpha channel value used for the color points");
    arg.param("vz_pointStep", pointStep, 1, "Step at which point are drawn");
    arg.param("vz_scale", scale, 2, "Depth image size reduction factor");
    arg.param("di_scaleFactor", di_scaleFactor, 0.001f, "Scale factor to apply to convert depth images in meters");
    arg.paramLeftOver("configFilename", configFilename, "", "Configuration filename", true);
    arg.paramLeftOver("logFilename", logFilename, "", "Log filename", true);
    arg.parseArgs(argc, argv);

    // Create GUI
    QApplication application(argc,argv);
    QWidget *mainWindow = new QWidget();
    mainWindow->setWindowTitle("pwn_tracker_log_viewer");
    QHBoxLayout *hlayout = new QHBoxLayout();
    mainWindow->setLayout(hlayout);
    QVBoxLayout *vlayout = new QVBoxLayout();
    hlayout->addItem(vlayout);
    QVBoxLayout *vlayout2 = new QVBoxLayout();
    hlayout->addItem(vlayout2);
    hlayout->setStretch(1, 1);

    QListWidget* listWidget = new QListWidget(mainWindow);
    listWidget->setSelectionMode(QAbstractItemView::MultiSelection);
    vlayout->addWidget(listWidget);
    PWNQGLViewer* viewer = new PWNQGLViewer(mainWindow);
    vlayout2->addWidget(viewer);
    Eigen::Isometry3f T;
    T.setIdentity();
    T.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f;

    // Read config file
    cout << "Loading config file..." << endl;
    Aligner *aligner;
    DepthImageConverter *converter;
    std::vector<Serializable*> instances = readConfig(aligner, converter, configFilename.c_str());
    converter->projector()->scale(1.0f/scale);
    cout << "... done" << endl;

    // Read and parse log file
    std::vector<boss::Serializable*> objects;
    std::vector<PwnTrackerFrame*> trackerFrames;
    std::vector<PwnTrackerRelation*> trackerRelations;
    Deserializer des;
    des.setFilePath(logFilename);
    cout << "Loading log file..." << endl;
    load(trackerFrames, trackerRelations, objects, des, step);
    cout << "... done" << endl;

    // Load the drawable list with the PwnTrackerFrame objects
    std::vector<Frame*> pointClouds;
    pointClouds.resize(trackerFrames.size());
    Frame *dummyFrame = 0;
    std::fill(pointClouds.begin(), pointClouds.end(), dummyFrame);
    for(size_t i = 0; i < trackerFrames.size(); i++) {
        PwnTrackerFrame *pwnTrackerFrame = trackerFrames[i];
        char nummero[1024];
        sprintf(nummero, "%05d", (int)i);
        listWidget->addItem(QString(nummero));
        QListWidgetItem *lastItem = listWidget->item(listWidget->count() - 1);

        Isometry3f transform = Isometry3f::Identity();
        if(applyTransform) {
            isometry3d2f(transform, pwnTrackerFrame->transform());
            transform = transform*pwnTrackerFrame->sensorOffset;
        }
        transform.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f;

        GLParameterFrame *frameParams = new GLParameterFrame();
        frameParams->setStep(pointStep);
        frameParams->setShow(false);
        DrawableFrame* drawableFrame = new DrawableFrame(transform, frameParams, pointClouds[i]);
        viewer->addDrawable(drawableFrame);
    }

    // Manage GUI
    viewer->init();
    mainWindow->show();
    viewer->show();
    listWidget->show();
    viewer->setAxisIsDrawn(true);
    bool selectionChanged = false;
    QListWidgetItem *item = 0;
    DepthImage depthImage;
    DepthImage scaledDepthImage;
    while (mainWindow->isVisible()) {
        selectionChanged = false;
        for (int i = 0; i<listWidget->count(); i++) {
            item = 0;
            item = listWidget->item(i);
            DrawableFrame *drawableFrame = dynamic_cast<DrawableFrame*>(viewer->drawableList()[i]);
            if (item && item->isSelected()) {
                if(!drawableFrame->parameter()->show()) {
                    drawableFrame->parameter()->setShow(true);
                    selectionChanged = true;
                }
                if(pointClouds[i] == 0) {
                    pointClouds[i] = new Frame();
                    boss_map::ImageBLOB* fromDepthBlob = trackerFrames[i]->depthImage.get();
                    DepthImage depthImage;
                    depthImage.fromCvMat(fromDepthBlob->cvImage());
                    DepthImage::scale(scaledDepthImage, depthImage, scale);
                    converter->compute(*pointClouds[i], scaledDepthImage);
                    drawableFrame->setFrame(pointClouds[i]);
                    delete fromDepthBlob;
                }
            } else {
                drawableFrame->parameter()->setShow(false);
                selectionChanged = true;
            }
        }
        if (selectionChanged)
            viewer->updateGL();

        application.processEvents();
    }

    return 0;
}
コード例 #13
0
int main (int argc, char** argv) {
  // Handle input
  int vz_applyTransform, vz_pointStep;
  int pwn_chunkStep;
  float vz_pointSize, vz_alpha;
  float pwn_scaleFactor, pwn_scale;

  string pwn_configFilename, pwn_logFilename, pwn_sensorType;
  string oc_benchmarkFilename, oc_trajectoryFilename, oc_groundTruthFilename, oc_associationsFilename;

  string directory;

  g2o::CommandArgs arg;
  arg.param("vz_pointSize", vz_pointSize, 1.0f, "Size value of the points to visualize");
  arg.param("vz_transform", vz_applyTransform, 1, "Apply absolute transform to the point clouds");
  arg.param("vz_alpha", vz_alpha, 1.0f, "Alpha channel value of the points to visualize");
  arg.param("vz_pointStep", vz_pointStep, 1, "Step value at which points are drawn");
  
  arg.param("pwn_chunkStep", pwn_chunkStep, 30, "Number of cloud composing a registered scene");
  arg.param("pwn_scaleFactor", pwn_scaleFactor, 0.001f, "Scale factor at which the depth images were saved");
  arg.param("pwn_scale", pwn_scale, 4.0f, "Scale factor the depth images are loaded");
  arg.param("pwn_configFilename", pwn_configFilename, "pwn.conf", "Specify the name of the file that contains the parameter configurations of the pwn structures");
  arg.param("pwn_logFilename", pwn_logFilename, "pwn.log", "Specify the name of the file that will contain the trajectory computed in boss format");
  arg.param("pwn_sensorType", pwn_sensorType, "kinect", "Sensor type: xtion640/xtion320/kinect/kinectFreiburg1/kinectFreiburg2/kinectFreiburg3");  
  arg.param("oc_benchmarkFilename", oc_benchmarkFilename, "gicp_benchmark.txt", "Specify the name of the file that will contain the results of the benchmark");
  arg.param("oc_trajectoryFilename", oc_trajectoryFilename, "gicp_trajectory.txt", "Specify the name of the file that will contain the trajectory computed");
  arg.param("oc_associationsFilename", oc_associationsFilename, "gicp_associations.txt", "Specify the name of the file that contains the images associations");
  arg.param("oc_groundTruthFilename", oc_groundTruthFilename, "groundtruth.txt", "Specify the name of the file that contains the ground truth trajectory");

  arg.paramLeftOver("directory", directory, ".", "Directory where the program will find the input needed files and the subfolders depth and rgb containing the images", true);

  arg.parseArgs(argc, argv);
  
  // Create GUI
  QApplication application(argc,argv);
  QWidget* mainWindow = new QWidget();
  mainWindow->setWindowTitle("GICP Odometry ");
  QHBoxLayout* hlayout = new QHBoxLayout();
  mainWindow->setLayout(hlayout);
  PWNQGLViewer* viewer = new PWNQGLViewer(mainWindow);
  hlayout->addWidget(viewer);
  viewer->init();
  viewer->setAxisIsDrawn(true);
  viewer->show();
  mainWindow->showMaximized();
  mainWindow->show();
  
  // Init odometry controller
  GICPOdometryController *gicpOdometryController = new GICPOdometryController(pwn_configFilename.c_str(), pwn_logFilename.c_str());
  gicpOdometryController->fileInitialization(oc_groundTruthFilename.c_str(), oc_associationsFilename.c_str(),
						      oc_trajectoryFilename.c_str(), oc_benchmarkFilename.c_str());
  gicpOdometryController->setScaleFactor(pwn_scaleFactor);
  gicpOdometryController->setScale(pwn_scale);
  gicpOdometryController->setSensorType(pwn_sensorType);
  gicpOdometryController->setChunkStep(pwn_chunkStep);


  // Compute odometry
  bool sceneHasChanged = false;
  Frame *frame = 0, *groundTruthReferenceFrame = 0;
  Isometry3f groundTruthPose;	
  GLParameterTrajectory *groundTruthTrajectoryParam = new GLParameterTrajectory(0.02f, Vector4f(0.0f, 1.0f, 0.0f, 0.6f));
  GLParameterTrajectory *trajectoryParam = new GLParameterTrajectory(0.02f, Vector4f(1.0f, 0.0f, 0.0f, 0.6f));
  std::vector<Isometry3f> trajectory, groundTruthTrajectory;
  std::vector<Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > trajectoryColors, groundTruthTrajectoryColors;
  DrawableTrajectory *drawableGroundTruthTrajectory = new DrawableTrajectory(Isometry3f::Identity(), 
									    groundTruthTrajectoryParam, 
									    &groundTruthTrajectory, 
									    &groundTruthTrajectoryColors);
  DrawableTrajectory *drawableTrajectory = new DrawableTrajectory(Isometry3f::Identity(), 
								 trajectoryParam, 
								 &trajectory, 
								 &trajectoryColors);
  viewer->addDrawable(drawableGroundTruthTrajectory);
  viewer->addDrawable(drawableTrajectory);
  bool finished = false;
  while (mainWindow->isVisible()) {
    sceneHasChanged = false;
    if (sceneHasChanged)
      viewer->updateGL();
    application.processEvents();

    if (finished) {
      continue;
    }
    
    if (gicpOdometryController->loadFrame(frame)) {
    
      if(gicpOdometryController->counter() == 1) {
	gicpOdometryController->getGroundTruthPose(groundTruthPose, atof(gicpOdometryController->timestamp().c_str()));
	// Add first frame to draw
	groundTruthReferenceFrame = new Frame();
	*groundTruthReferenceFrame = *frame;
	GLParameterFrame *groundTruthReferenceFrameParams = new GLParameterFrame();
	groundTruthReferenceFrameParams->setStep(vz_pointStep);
	groundTruthReferenceFrameParams->setShow(true);	
	groundTruthReferenceFrameParams->parameterPoints()->setColor(Vector4f(1.0f, 0.0f, 1.0f, vz_alpha));
	DrawableFrame *drawableGroundTruthReferenceFrame = new DrawableFrame(groundTruthPose, groundTruthReferenceFrameParams, groundTruthReferenceFrame);
	GLParameterFrame *frameParams = new GLParameterFrame();
	frameParams->setStep(vz_pointStep);
	frameParams->setShow(true);	
	DrawableFrame *drawableFrame = new DrawableFrame(gicpOdometryController->globalPose(), frameParams, frame);
	viewer->addDrawable(drawableGroundTruthReferenceFrame);
	viewer->addDrawable(drawableFrame);

	sceneHasChanged = true;
      }
      // Compute current transformation
      if (gicpOdometryController->processFrame()) {
	gicpOdometryController->getGroundTruthPose(groundTruthPose, atof(gicpOdometryController->timestamp().c_str()));	
	
	// Add frame to draw
	GLParameterFrame *frameParams = new GLParameterFrame();
	frameParams->setStep(vz_pointStep);
	frameParams->setShow(true);	
	DrawableFrame *drawableFrame = new DrawableFrame(gicpOdometryController->globalPose(), frameParams, frame);
	viewer->addDrawable(drawableFrame);
	
	// Add trajectory pose
	groundTruthTrajectory.push_back(groundTruthPose);
	groundTruthTrajectoryColors.push_back(Vector4f(0.0f, 1.0f, 0.0f, 0.6f));
	trajectory.push_back(gicpOdometryController->globalPose());
	trajectoryColors.push_back(Vector4f(1.0f, 0.0f, 0.0f, 0.6f));
	drawableGroundTruthTrajectory->updateTrajectoryDrawList();
	drawableTrajectory->updateTrajectoryDrawList();
      
	// Write results
	gicpOdometryController->writeResults();
      
	sceneHasChanged = true;
      }
 
      // Remove old frames
      if (viewer->drawableList().size() > 23) {
	DrawableFrame *d = dynamic_cast<DrawableFrame*>(viewer->drawableList()[3]);
	if (d) {
	  Frame *f = d->frame();
	  if (gicpOdometryController->referenceFrame() != f &&
	      gicpOdometryController->currentFrame() != f) {
	    viewer->erase(3);
	    delete d;
	    delete f; 
	  }
	}
      }

      frame = 0;
    }
    else {
      finished = true;
    }
  }

  return 0;
}