Exemplos de draw3DMap em C++ (Cpp)

Exemplo n.º 1

Arquivo: main.cpp Projeto: oakfr/omni3d

void MyGlWindow::draw()
{	// the draw method must be private

	glClearColor(0,0,0,0);		// clear the window to black
	glClear(GL_COLOR_BUFFER_BIT);	// clear the window

	showAndHide();
	glDisable(GL_LIGHTING);

	switch (_windowMode) {
	case MODE_VIDEO:
		drawVideo(false);
		break;
	case MODE_3D_MAP:
		glEnable(GL_LIGHTING);
		draw3DMap();
		break;
	case MODE_2D_MAP:
		draw2DMap();
		break;
	case MODE_SPHERE:
		drawSphere();
		break;
	case MODE_CALIBRATION:
		drawCalibration();
		break;
	case MODE_ROTATIONS:
		drawRotations();
		break;
	default:
		exit(0);
	}
	
	//if ( snapshot->value() )
	//	drawSnapshotScreen();
	//redraw();
};

Exemplo n.º 2

Arquivo: main.cpp Projeto: chentyjpm/OpenEKFMonoSLAM

int main(int argc, const char * argv[])
{
    Timer timer;
    timer.start();

    FileSequenceImageGenerator generator(argv[2], "", "png", 90, 6550);

    //VideoFileImageGenerator generator(argv[2]);

    cv::Mat image;

    generator.init();

    // path al archivo de configuracion, path de salida
    std::string outputPath("");
    if (argc > 3)
    {
        outputPath = argv[3];
    }
    else
    {
        outputPath = "";
    }

    EKF extendedKalmanFilter(argv[1], outputPath.c_str());

    // para la primer imagen, se inicializa Kalman Filter
    // Se hace un sensado y se agregan todos los features que se detectan en la imagen.
    image = generator.getNextImage();

#ifdef DEBUG
    int stepCount = 0;
    std::cout << "~~~~~~~~~~~~ STEP 0 ~~~~~~~~~~~~" << std::endl;
#endif

    if (!image.empty())
    {
        extendedKalmanFilter.init(image);
    }
    else
    {
        std::cout << "No se puede iniciar Kalman Filter dado que no hay imagenes disponibles." << std::endl;
        return 0;
    }

#ifdef DEBUG
    std::cout << extendedKalmanFilter.state << std::endl;
#endif

#ifdef DEBUG_SHOW_3D_MAP
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = create3DMapVisualizer(extendedKalmanFilter.state);
#endif

#ifdef DEBUG_SHOW_PLANAR_INFO
    double cameraPreviousPosition[3];

    cameraPreviousPosition[0] = state.position[0];
    cameraPreviousPosition[1] = state.position[1];
    cameraPreviousPosition[2] = state.position[2];
    // Se crean las ventanas y se muestran las posciones
    std::string windowName = "Informacion planar";
    cv::Size size(PLANAR_WINDOW_SIZE_X, PLANAR_WINDOW_SIZE_Y);

    cv::Mat cameraPositionImage(cv::Mat::zeros(size, CV_8UC3));
    cv::Mat cameraPositionEllipse(cv::Mat::zeros(size, CV_8UC3));
    cv::Mat featuresPositionImage(cv::Mat::zeros(size, CV_8UC3));
    cv::namedWindow(windowName);

    std::string windowNameActualImage = "Imagen actual";
    //cv::namedWindow(windowNameActualImage);

    drawPlanarInformation(state, covariance, cameraPreviousPosition, PLANAR_INFORMATION_XY, cameraPositionImage, featuresPositionImage, cameraPositionEllipse);

    cv::imshow(windowName, cv::Mat(cameraPositionImage + featuresPositionImage));
//    cv::imshow(windowNameActualImage, image);
    cv::waitKey(0);
#endif

    image = generator.getNextImage();

    while(!image.empty())
    {
#ifdef DEBUG
        std::cout << std::endl << std::endl;
        std::cout << "~~~~~~~~~~~~ STEP " << stepCount++ << " ~~~~~~~~~~~~" << std::endl;
#endif

        // para cada imagen recibida se hace un paso del EKF
        extendedKalmanFilter.step(image);

#ifdef DEBUG
        // mostramos el estado
        std::cout << extendedKalmanFilter.state << std::endl;
#endif

#ifdef DEBUG_SHOW_3D_MAP
        draw3DMap(extendedKalmanFilter.state, viewer);
        viewer->spinOnce(100);
#endif
#ifdef DEBUG_SHOW_PLANAR_INFO
        cv::Mat(cv::Mat::zeros(size, CV_8UC3)).copyTo(featuresPositionImage);
        cv::Mat(cv::Mat::zeros(size, CV_8UC3)).copyTo(cameraPositionEllipse);

        // Se muestra la posicion en 2 dimensiones.
        drawPlanarInformation(state, covariance, cameraPreviousPosition, PLANAR_INFORMATION_XY, cameraPositionImage, featuresPositionImage, cameraPositionEllipse);

        cv::imshow(windowName, cv::Mat(cameraPositionImage + featuresPositionImage + cameraPositionEllipse));
        cv::waitKey(1);
//        cv::imshow(windowNameActualImage, image);

        // Se actualiza la posicion anterior de la camara
        cameraPreviousPosition[0] = state.position[0];
        cameraPreviousPosition[1] = state.position[1];
        cameraPreviousPosition[2] = state.position[2];
#endif

#if defined(DEBUG_SHOW_IMAGES)
        // waitKey global para ir paso a paso o hacer todo de corrido
        cv::waitKey(1);
#endif

        image = generator.getNextImage();
    }

    std::cout << "Tiempo de ejecución total: "<< timer.getElapsedTimeInMilliSec() << " ms" << std::endl;
    timer.stop();

#if defined(DEBUG_SHOW_IMAGES)
    cv::destroyAllWindows();
#endif

    return 0;
}