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;
}
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();;
}
}
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;
}