Esempio n. 1
0
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;
}
Esempio n. 2
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;
}