int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kError);
if(argc < 8)
{
showUsage();
}
int argIndex = 1;
int cameraRate = atoi(argv[argIndex++]);
if(cameraRate <= 0)
{
printf("camera_rate should be > 0\n");
showUsage();
}
int odomUpdate = atoi(argv[argIndex++]);
if(odomUpdate <= 0)
{
printf("odom_update should be > 0\n");
showUsage();
}
int mapUpdate = atoi(argv[argIndex++]);
if(mapUpdate <= 0)
{
printf("map_update should be > 0\n");
showUsage();
}
printf("Camera rate = %d Hz\n", cameraRate);
printf("Odometry update rate = %d Hz\n", cameraRate/odomUpdate);
printf("Map update rate = %d Hz\n", (cameraRate/odomUpdate)/mapUpdate);
std::string calibrationDir = argv[argIndex++];
std::string calibrationName = argv[argIndex++];
std::string pathLeftImages = argv[argIndex++];
std::string pathRightImages = argv[argIndex++];
Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
CameraStereoImages camera(
pathLeftImages,
pathRightImages,
false, // assume that images are already rectified
(float)cameraRate,
opticalRotation);
if(camera.init(calibrationDir, calibrationName))
{
OdometryF2M odom;
Rtabmap rtabmap;
rtabmap.init();
QApplication app(argc, argv);
MapBuilder mapBuilder;
mapBuilder.show();
QApplication::processEvents();
SensorData data = camera.takeImage();
int cameraIteration = 0;
int odometryIteration = 0;
printf("Press \"Space\" in the window to pause\n");
while(data.isValid() && mapBuilder.isVisible())
{
if(cameraIteration++ % odomUpdate == 0)
{
OdometryInfo info;
Transform pose = odom.process(data, &info);
if(odometryIteration++ % mapUpdate == 0)
{
if(rtabmap.process(data, pose))
{
mapBuilder.processStatistics(rtabmap.getStatistics());
if(rtabmap.getLoopClosureId() > 0)
{
printf("Loop closure detected!\n");
}
}
}
mapBuilder.processOdometry(data, pose, info);
}
QApplication::processEvents();
while(mapBuilder.isPaused() && mapBuilder.isVisible())
{
uSleep(100);
QApplication::processEvents();
}
data = camera.takeImage();
}
if(mapBuilder.isVisible())
{
printf("Processed all frames\n");
app.exec();
}
}
else
{
UERROR("Camera init failed!");
}
return 0;
}
int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
// GUI stuff, there the handler will receive RtabmapEvent and construct the map
QApplication app(argc, argv);
MapBuilder mapBuilder;
// Here is the pipeline that we will use:
// CameraOpenni -> "CameraEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"
// Create the OpenNI camera, it will send a CameraEvent at the rate specified.
// Set transform to camera so z is up, y is left and x going forward
CameraOpenni camera("", 10, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
if(!camera.init())
{
UERROR("Camera init failed!");
exit(1);
}
// Create an odometry thread to process camera events, it will send OdometryEvent.
OdometryThread odomThread(new OdometryBOW(0)); // 0=SURF 1=SIFT
// Create RTAB-Map to process OdometryEvent
Rtabmap * rtabmap = new Rtabmap();
rtabmap->init();
RtabmapThread rtabmapThread(rtabmap); // ownership is transfered
// Setup handlers
odomThread.registerToEventsManager();
rtabmapThread.registerToEventsManager();
mapBuilder.registerToEventsManager();
// The RTAB-Map is subscribed by default to CameraEvent, but we want
// RTAB-Map to process OdometryEvent instead, ignoring the CameraEvent.
// We can do that by creating a "pipe" between the camera and odometry, then
// only the odometry will receive CameraEvent from that camera. RTAB-Map is
// also subscribed to OdometryEvent by default, so no need to create a pipe between
// odometry and RTAB-Map.
UEventsManager::createPipe(&camera, &odomThread, "CameraEvent");
// Let's start the threads
rtabmapThread.start();
odomThread.start();
camera.start();
mapBuilder.show();
app.exec(); // main loop
// remove handlers
mapBuilder.unregisterFromEventsManager();
rtabmapThread.unregisterFromEventsManager();
odomThread.unregisterFromEventsManager();
// Kill all threads
camera.kill();
odomThread.join(true);
rtabmapThread.join(true);
return 0;
}
int main(int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
ParametersMap pm = Parameters::parseArguments(argc, argv);
pm.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), "."));
int argIndex = 1;
int cameraRate = atoi(argv[argIndex++]);
if(cameraRate <= 0)
{
printf("camera_rate should be > 0\n");
showUsage();
}
int odomUpdate = atoi(argv[argIndex++]);
if(odomUpdate <= 0)
{
printf("odom_update should be > 0\n");
showUsage();
}
int mapUpdate = atoi(argv[argIndex++]);
if(mapUpdate <= 0)
{
printf("map_update should be > 0\n");
showUsage();
}
printf("Camera rate = %d Hz\n", cameraRate);
printf("Odometry update rate = %d Hz\n", cameraRate/odomUpdate);
printf("Map update rate = %d Hz\n", (cameraRate/odomUpdate)/mapUpdate);
std::string calibrationDir = argv[argIndex++];
std::string calibrationName = argv[argIndex++];
std::string pathRGBImages = argv[argIndex++];
std::string pathDepthImages = argv[argIndex++];
Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
//CameraStereoImages camera(
// pathLeftImages,
// pathRightImages,
// false, // assume that images are already rectified
// (float)cameraRate,
// opticalRotation);
// CameraFreenect2 camera(0, CameraFreenect2::Type::kTypeColor2DepthSD, 0.0, opticalRotation);
CameraRGBDImages camera(pathRGBImages, pathDepthImages, 1, float(cameraRate), opticalRotation);
std::ofstream stats_file;
stats_file.open("statistics.txt");
stats_file << "[" << std::endl;
if(camera.init(calibrationDir, calibrationName))
{
OdometryF2M odom;
Rtabmap rtabmap;
rtabmap.init(pm);
QApplication app(argc, argv);
MapBuilder mapBuilder;
mapBuilder.show();
QApplication::processEvents();
SensorData data = camera.takeImage();
int cameraIteration = 0;
int odometryIteration = 0;
printf("Press \"Space\" in the window to pause\n");
while(data.isValid() && mapBuilder.isVisible())
{
if(cameraIteration++ % odomUpdate == 0)
{
OdometryInfo info;
Transform pose = odom.process(data, &info);
if(odometryIteration++ % mapUpdate == 0)
{
if(rtabmap.process(data, pose))
{
mapBuilder.processStatistics(rtabmap.getStatistics());
if(rtabmap.getLoopClosureId() > 0)
{
printf("Loop closure detected!\n");
}
std::map<std::string, float> statz = rtabmap.getStatistics().data();
stats_file << " {" << std::endl;
for(std::map<std::string,float>::iterator it = statz.begin(); it != statz.end(); ++it) {
std::string name = it->first;
std::replace(name.begin(), name.end(), '/', '.');
stats_file << " \"" << name << "\": " << it->second << "," << std::endl;
}
stats_file << " }," << std::endl;
}
}
mapBuilder.processOdometry(data, pose, info);
}
QApplication::processEvents();
while(mapBuilder.isPaused() && mapBuilder.isVisible())
{
uSleep(100);
QApplication::processEvents();
}
data = camera.takeImage();
}
if(mapBuilder.isVisible())
{
printf("Processed all frames\n");
app.exec();
}
}
else
{
UERROR("Camera init failed!");
}
stats_file << "]" << std::endl;
stats_file.close();
return 0;
}