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::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;
}
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;
}
