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