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;
}
int main(int argc, char **argv) {
float pointSize;
float pointStep;
float normalLenght;
float normalStep;
float alphaColor;
int initialSensorOffset;
string filename;
// Input parameters handling.
g2o::CommandArgs arg;
arg.param("pointSize", pointSize, 1.0f, "Size of the points");
arg.param("normalLenght", normalLenght, 0, "Lenght of the normals");
arg.param("alpha", alphaColor, 1.0f, "Alpha channel for points");
arg.param("pointStep", pointStep, 1, "Step of the points");
arg.param("normalStep", normalStep, 1, "Step of the normals");
arg.param("initialSensorOffset", initialSensorOffset, 0, "Choose if use the initial sensor offset");
arg.paramLeftOver("filename", filename, "", "Input depth image or .pwn file", true);
// Set parser input.
arg.parseArgs(argc, argv);
Isometry3f sensorOffsetInit = Isometry3f::Identity();
if(initialSensorOffset) {
sensorOffsetInit.translation() = Vector3f(0.15f, 0.0f, 0.05f);
Quaternionf quaternion = Quaternionf(0.5f, -0.5f, 0.5f, -0.5f);
sensorOffsetInit.linear() = quaternion.toRotationMatrix();
}
sensorOffsetInit.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f;
// Windows name
static const char CALIBRATE_WINDOW[] = "Calibrate Window";
// Calibration angles
int maxTrackBarValue = 1800;
int alpha = maxTrackBarValue / 2; // Around x
int beta = maxTrackBarValue / 2; // Around y
int theta = maxTrackBarValue / 2; // Around z
// Create the window and the trackbars
cv::namedWindow(CALIBRATE_WINDOW);
cvMoveWindow(CALIBRATE_WINDOW, 0, 0);
cvResizeWindow(CALIBRATE_WINDOW, 500, 200);
cvCreateTrackbar("Calibrate X", CALIBRATE_WINDOW, &alpha, 1800, NULL);
cvCreateTrackbar("Calibrate Y", CALIBRATE_WINDOW, &beta, 1800, NULL);
cvCreateTrackbar("Calibrate Z", CALIBRATE_WINDOW, &theta, 1800, NULL);
// Create objects in order to read the input depth image / pwn file
Eigen::Matrix3f cameraMatrix;
cameraMatrix <<
525.0f, 0.0f, 319.5f,
0.0f, 525.0f, 239.5f,
0.0f, 0.0f, 1.0f;
string extension;
extension = filename.substr(filename.rfind(".") + 1);
Frame frame;
Isometry3f globalT = Isometry3f::Identity();
cerr << "Loading " << filename.c_str() << endl;
if(extension == "pgm") {
PinholePointProjector projector;
projector.setCameraMatrix(cameraMatrix);
StatsCalculator statsCalculator;
PointInformationMatrixCalculator pointInformationMatrixCalculator;
NormalInformationMatrixCalculator normalInformationMatrixCalculator;
DepthImageConverter depthImageConverter(&projector, &statsCalculator,
&pointInformationMatrixCalculator,
&normalInformationMatrixCalculator);
DepthImage inputImage;
inputImage.load(filename.c_str(),true);
cerr << "Computing stats... ";
depthImageConverter.compute(frame, inputImage);
cerr << " done" << endl;
}
else if(extension == "pwn") {
frame.load(globalT, filename.c_str());
}
else {
cerr << "File extension nor recognized, quitting." << endl;
return(0);
}
Isometry3f oldSensorOffset = Isometry3f::Identity();
oldSensorOffset.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f;
QApplication application(argc,argv);
QWidget* mainWindow = new QWidget();
mainWindow->setWindowTitle("pwn_calibration");
QHBoxLayout* hlayout = new QHBoxLayout();
mainWindow->setLayout(hlayout);
QVBoxLayout* vlayout = new QVBoxLayout();
hlayout->addItem(vlayout);
PWNQGLViewer* viewer = new PWNQGLViewer(mainWindow);
vlayout->addWidget(viewer);
viewer->init();
viewer->show();
viewer->setAxisIsDrawn(true);
mainWindow->show();
GLParameterPoints *pointsParams = new GLParameterPoints(pointSize, Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f));
pointsParams->setStep(pointStep);
DrawablePoints *drawablePoints = new DrawablePoints(oldSensorOffset, pointsParams, &frame.points(), &frame.normals());
viewer->addDrawable(drawablePoints);
GLParameterNormals *normalParams = new GLParameterNormals(pointSize, Eigen::Vector4f(0.0f, 0.0f, 1.0f, alphaColor), normalLenght);
DrawableNormals *drawableNormals = new DrawableNormals(oldSensorOffset, normalParams, &frame.points(), &frame.normals());
normalParams->setStep(normalStep);
normalParams->setNormalLength(normalLenght);
viewer->addDrawable(drawableNormals);
// Keep cycling
Isometry3f sensorOffset;
while(mainWindow->isVisible()) {
// Updating variables
float alphar = 2.0f * 3.14*((float)alpha - maxTrackBarValue / 2) / maxTrackBarValue;
float betar = 2.0f * 3.14*((float)beta - maxTrackBarValue / 2) / maxTrackBarValue;
float thetar = 2.0f * 3.14*((float)theta - maxTrackBarValue / 2) / maxTrackBarValue;
// Generate rotations
Quaternion<float> qx, qy, qz;
qx = AngleAxis<float>(alphar, Vector3f(1.0f, 0.0f, 0.0f));
qy = AngleAxis<float>(betar, Vector3f(0.0f, 1.0f, 0.0f));
qz = AngleAxis<float>(thetar, Vector3f(0.0f, 0.0f, 1.0f));
Matrix3f totalRotation = qz.toRotationMatrix() * qy.toRotationMatrix() * qx.toRotationMatrix();
sensorOffset = Isometry3f::Identity();
sensorOffset.translation() = Vector3f(0.0f, 0.0f, 0.0f);
sensorOffset.linear() = totalRotation * sensorOffsetInit.linear();
sensorOffset.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f;
drawablePoints->setTransformation(sensorOffset);
drawableNormals->setTransformation(sensorOffset);
oldSensorOffset = sensorOffset;
viewer->updateGL();
application.processEvents();
cv::waitKey(33);
}
ofstream os("sensorOffset.txt");
Vector6f offset = t2v(sensorOffset);
os << offset[0] << " " << offset[1] << " " << offset[2] << " "
<< offset[3] << " " << offset[4] << " " << offset[5] << endl;
return(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;
}
