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