/home/jgoppert/Projects/uvsim/uvsim/src/test/stereoMapping.cc

Go to the documentation of this file.

/*
 * stereoMapping.cc
 * Copyright (C) Brandon Wampler <bwampler@purdue.edu>
 *
 * stereoMapping.cc is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * stereoMapping.cc is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <iostream>
#include <ctime>
#include <string>

#include <osg/PositionAttitudeTransform>
#include <osgGA/TrackballManipulator>
#include <osg/Geode>
#include <osg/ShapeDrawable>
#include <osg/Geometry>
#include <osg/PointSprite>
#include <osg/Point>

#include "uvsim/visualization/PointCloud.h"
#include "uvsim/visualization/SimpleViewer.h"
#include "uvsim/sensing/StereoEngine.h"
#include "uvsim/sensing/Camera.h"
#include "uvsim/navigation/Ins.h"
#include "uvsim/navigation/GeoMag.h"
#include "uvsim/utilities/utilities.h"

using namespace boost::numeric::ublas;
int main (int argc, char const* argv[])
{
//Initialize Stereo Vision
    //uvsim::Camera cameraL(0);
    //uvsim::Camera cameraR(1);
    //uvsim::StereoEngine *stereo = new uvsim::StereoEngine(cameraL.m_frame, cameraR.m_frame);
        uvsim::StereoEngine *stereo = new uvsim::StereoEngine(cvLoadImage("data/01L.jpg"),cvLoadImage("data/01R.jpg"));
    cvNamedWindow("leftVideo", CV_WINDOW_AUTOSIZE);
    cvNamedWindow("rightVideo", CV_WINDOW_AUTOSIZE);

    // constants
    const static double refreshRate = 0.05;
    const static double depthRate = 2.0;
    const static int freq = 500;

    // cutoff frequencies
    vector<double> freqCutMb(3), freqCutFb(3), freqCutWb(3);

    // magnetometer cutoff freq
    freqCutMb(0) = 1; //312;
    freqCutMb(1) = 1; //312;
    freqCutMb(2) = 1; //312;

    // accel cutoff freq
    freqCutFb(0) = 350; //350;
    freqCutFb(1) = 350; //350;
    freqCutFb(2) = 150; //150;

    // gyro cutoff freq
    freqCutWb(0) = 120; //120;
    freqCutWb(1) = 120; //120;
    freqCutWb(2) = 120; //120;

    // variables
    float dec, dip, ti, gv;

    // Purdue airport
    float alt, glat, glon, time;
    alt = 184.7; // m
    glat = 40.41194444; // deg
    glon = -86.93361111; // deg
    time = 2009.7; // decimal yearsa

    // test ins
        //uvsim::Imu6DofSensor imu("/dev/rfcomm0", freqCutMb, freqCutFb, freqCutWb);
        //uvsim::Ins *ins = new uvsim::Ins(std::string(DATADIR)+="/data/Ins15g.dat",
        //std::string(DATADIR)+="/data/Gyro.dat",
        //std::string(DATADIR)+="/data/Mag.dat",freq,&imu);
        //ins->initializeStatic(alt,glat,glon);

    // osg
    vector<double> pos(3);

    osg::Vec3Array *points = new osg::Vec3Array;
    osg::Vec4Array *colors = new osg::Vec4Array;
    osg::Vec4 ini(1,1,0,1);
    osg::Vec4 fin(0,0,1,1);
        uvsim::PointCloud *pointCloud = new uvsim::PointCloud(1,points,colors);
    osg::Group *root = new osg::Group;
    root->addChild((pointCloud->root).get());

    // Launch the viewer thread
        int fps = 20;
        osg::Vec3d manipCenter(0,0,0);
        double manipDistance = 10;
        float frameSize = 1;
        double fogStart=0, fogEnd=1000;
        uvsim::SimpleViewer simpleViewer(0,fps,root,manipDistance,manipCenter,frameSize,fogStart,fogEnd);
        simpleViewer.start();

        vector<double> bodyCoord(3), worldCoord(3);
            // loop
    for (int i=0; !simpleViewer.viewer->done(); i++)
    {
        if ((i % int(freq*depthRate)) == 0)
        {
                        //update depth engine
                        //cameraL.update();
                        //cameraR.update();
            stereo->findDisparity();
            stereo->reprojectTo3d();
                        std::cout<<"Depth Update"<<std::endl;
                        points->clear();
                        colors->clear();

            // display to terminal
                        //ins->print();

                        //update visual
                        
                        //quat(0) = 1;
                        //quat(1) = 0;
                        //quat(2) = 0;
                        //quat(3) = 0;
                        //vector<double> *quat = &ins->getQuat();
                        //cout<<"************************************quat- "<<quat<<endl;
                        //pos = ins.getGeo().getCartesian();
                

            for ( int y=0; y<stereo->threeD->height; y++)
            {
                float* ptr = (float*) (stereo->threeD->imageData + y * stereo->threeD->widthStep);
                                uint8_t* color_ptr = (uint8_t*) (stereo->leftImageR->imageData + y * stereo->leftGreyR->widthStep);

                for (int x=0; x<stereo->threeD->width; x++)
                {

                    if ((CV_MAT_ELEM(*stereo->disp, signed short,y,x) !=  (stereo->BMState->minDisparity - 1) * 16)  && (abs(ptr[3*x+2])< 20))
                    { 
                                                //cout<<"X- "<<ptr[3*x+0]<<"   Y- "<<ptr[3*x+1]<<"   Z- "<<ptr[3*x+2]<<endl;
                                                bodyCoord(0) = -ptr[stereo->threeD->nChannels*x+2];
                                                bodyCoord(1) = -ptr[stereo->threeD->nChannels*x+0];
                                                bodyCoord(2) = -ptr[stereo->threeD->nChannels*x+1];
                                                
                                                std::cout<<"bodyCoord- "<<bodyCoord<<std::endl;
                                                vector<double> quat = zero_vector<double>(4);
                                                quat(0) = 1;
                                                quat(2) = 1;
                                                worldCoord = uvsim::quatRotate(quat,bodyCoord);
                                        
                                                //worldCoord = worldCoord /*+ pos*/;
                                                //std::cout<<"R- "<<color_ptr[3*x+0]<<"   G- "<<color_ptr[3*x+1]<<"   B- "<<color_ptr[3*x+2]<<std::endl;

                                                points->push_back(osg::Vec3(worldCoord(0), worldCoord(1), worldCoord(2) ));
                                                colors->push_back(osg::Vec4(color_ptr[3*x+0], color_ptr[3*x+1], color_ptr[3*x+2],1));

                    }


                }
            }
                        pointCloud->updateSize();
        }
                //ins->update();
    }
        //ins->stop();
        simpleViewer.join();
    return(0);

}
// vim:ts=4:sw=4

---