예제 #1
0
 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;
 }
예제 #2
0
  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();;
    } 
  }
예제 #3
0
  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;
  }