openMVG: "open Multiple View Geometry"

"open Multiple View Geometry" is a library for computer-vision scientists and especially targeted to the Multiple View Geometry community. It is designed to provide an easy access to the classical problem solvers in Multiple View Geometry and solve them accurately.

The openMVG credo is: "Keep it simple, keep it maintainable". OpenMVG targets readable code that is easy to use and modify by the community.

All the features and modules are unit tested. This test driven development ensures that the code works as it should and enables more consistent repeatability. Furthermore, it makes it easier for the user to understand and learn the given features.

Core features

openMVG multiview module consists of a collection of:

• solvers for 2 to n-view geometry constraints that arise in multiple view geometry.
• a generic framework that can embed these solvers for robust estimation.

openMVG provides a complete Structure from Motion incremental chain.

Core features

openMVG multiview module consists of a collection of:

• solvers for 2 to n-view geometry constraints that arise in multiple view geometry.
• Homography
• Fundamental matrix
• Essential matrix
• Resection/Pose estimation matrix
• Triangulation


• a generic framework that can embed these solvers for robust estimation
• Max-Consensus
• Ransac
• AC-Ransac (model and precision estimation)
  


• an easy access to powerful library:
• Matrix, vector operations and linear algebra: Eigen library
• Nonlinear Least Squares Minimizer: Google Ceres-solver
• Graph management and operation: Lemon library

Ceres Solver

Ceres Solver

Solving nonlinear least squares problems comes up in a broad range of areas across science and engineering - from fitting curves in statistics, to constructing 3D models from photographs in computer vision. Ceres Solver is a portable C++ library that allows for modeling and solving large complicated nonlinear least squares problems.

Features

• A friendly API: build your objective function one term at a time.
• Automatic differentiation.
• Robust loss functions.
• Local parameterizations.
• Threaded Jacobian evaluators and linear solvers.
• Levenberg-Marquardt and Dogleg (Powell & Subspace) solvers.
• Dense QR and Cholesky factorization (using Eigen) for small problems.
• Sparse Cholesky factorization (using SuiteSparse) for large sparse problems.
• Specialized solvers for bundle adjustment problems in computer vision.
• Iterative linear solvers for general sparse and bundle adjustment problems.
• Runs on Linux, Windows, Mac OS X and Android. An iOS port is underway.

Please see the manual for a tutorial introduction and further details.

LEMON

Welcome to COIN-OR::LEMON

LEMON stands for Library for Efficient Modeling and Optimization in Networks. It is a C++ template library providing efficient implementations of common data structures and algorithms with focus on combinatorial optimization tasks connected mainly with graphs and networks.

LEMON is a member of the COIN-OR initiative, a collection of OR related open source projects. You are free to use it in your commercial or non-commercial applications under very permissive license terms.

https://lemon.cs.elte.hu/trac/lemon

Explaining the alternatives for UAV navigation by Jerry LeMieux

Explaining the alternatives for UAV navigation by Jerry LeMieux

UAVs use a number of navigation systems to fly a path to an assigned area. The generated coordinates are also used to geo-locate the UAV position and its imagery, making it a very important part of the Unmanned Aircraft System (UAS).

One method of navigation is by using an inertial navigation system (INS). An INS can have many different architectures, sizes and degrees of precision. Most INS’s use accelerometers to determine acceleration and gyroscopes to determine angular rotation. These accelerometers and gyroscopes are packaged together into an inertial measurement unit (IMU). An INS can determine position, orientation and angular velocity using only the IMU readings and elapsed time. Normally, an initial known position is loaded on startup to align the INS system. The problem with an INS is that over time, measurement accuracy degrades and errors accumulate. Typically, the smaller the INS, the larger the decrease in accuracy. Sometimes the INS has bad estimates of position that are far from a previous estimate.

Many countries have developed independent satellite navigation systems that are more accurate than INS. The USA has GPS, Europe has Galileo, Russia has the Global Navigation Satellite System (GLONASS), China has Beidou or COMPASS, India has the Indian Regional Navigation Satellite System (IRNSS), France has the Doppler Orbitography and Radio (DORIS) and Japan has the Quazi Zenith Satellite System (QZSS). Collectively, they are all called the Global Navigation Satellite System. These satellite systems are normally found in low earth orbit with orbital periods of 10 to 15 hours. This orbit causes the geometry of the satellites to constantly change. Since a GPS position estimate is a solution to a triangulation problem, the accuracy depends on the geometry. Poor geometry means a poor estimate of position.

Other problems with satellite navigation are that the signal is very weak; the signal can be easily interfered with or jammed; and the signal cannot penetrate buildings and is subject to atmospheric interference. There are also many sources of error introduced into the satellite position estimate. The ionosphere and troposphere disturbances cause the satellite signal to slow down as it passes through the atmosphere. Signals hit objects and are reflected off.  This causes the signal to be delayed before it reaches the receiver. Another name for this phenomenon is called multipath. The clock inside the satellite receiver is not as accurate as an atomic clock and slight timing errors lead to errors in calculations. It is possible to correct for these errors using a differential signal. If a position on the ground is known exactly it can be compared to a satellite navigation signal estimate. The error between these positions can be calculated and used to correct the satellite navigation signal.

A satellite navigation position estimate is normally very good and much better than INS. Sometimes the signal is poor due to bad satellite geometry. Sometimes the satellite navigation signal is not available due to interference or jamming. When flying a UAV in a valley or in a city, the satellite signals can be blocked and the signal is not available. So what can we do when the satellite signal is blocked? One of the most common methods used to is to combine a satellite navigation receiver with an INS. During the times that the satellite signal is not available, the INS takes over. However, again, the INS accumulates errors over time and may not be the best choice. So, in an integrated satellite/INS solution, how do we choose which signal will be used. The answer is to use a data fusion algorithm that will pick the most accurate estimate of position. The most common type of fusion algorithm is called the Kalman filter. The Kalman filter tracks both the INS and satellite system position estimates over time. If an estimate is very far off from a previous estimate it is rejected. The filter tracks previous estimates from both the INS and satellite navigation system and uses these calculations to project the next point. The actual next point is measured and an error is computed. Each sensor estimate is assigned a weight. The estimate with the highest value is selected as the final solution.

Some are concerned about the possibility of satellite navigation signal spoofing. This occurs when a strong signal from the ground is used to take over the navigation of the UAV. A signal generated from the earth will be much stronger than a satellite based signal. However, very precise timing and position updating must be incorporated to accomplish a takeover. There are technologies today that will counter this problem such as commercially available anti-spoofing systems. These systems can detect the stronger signal and reject it. Other methods can be used to encrypt the signal with a code. Unless the spoofer knows the code, it will not be possible to take control.

The majority of UAV systems have come to rely heavily on satellite based navigation. These systems are vulnerable if the satellite signal is lost, countermeasures are applied or the receiver fails. During the last war in Operation Iraqi Freedom (OIF), many small satellite jammers were used to deny navigation. In addition, many countries have demonstrated the capability to disable satellites. Future conflicts may be fought in low altitude urban environments where satellite navigation signals will be blocked. So we must find alternative navigation systems that are more reliable and less susceptible to interference and jamming. There are currently many types of systems under development or in production. Image aided navigation, video navigation, sun trackers, star trackers, use of signals of opportunity and using very precise INS are some of the solutions I have discovered in my research. Now I would like to describe how some of these systems operate.

Before we discuss alternative systems it is important to note that data fusion algorithms like the Kalman filter can be used with two or more sensors. The difference between the predicted feature location and the actual feature location can be used to resolve IMU biases. Typically, when multiple navigation system position estimates are combined, the INS becomes a core component. If other sensor system estimates are more accurate than the INS, they can be used to correct or update the INS.

Image aided navigation uses the sensor images to navigate. Measurements of 3D terrain imagery can be obtained from EO/IR sensors. To navigate with images, a Digital Terrain Elevation Data (DTED) map database can be stored in memory. The sensor images can be correlated with the stored DTED map and an error can be computed. This error can be used to update an INS. The Kalman filter can fuse imagery and INS measurements by using the results from feature correspondence to correct the INS. Trajectory corrections are then fed back to improve future predictions. Another method uses the imagery to compute a new coordinate. The new coordinate is then uploaded to the UAV memory and used for navigation.

Another alternative navigation system is called Locata. This is basically a satellite navigation system installed on a series of ground based towers. It is a ground based positioning system with centimeter accuracy. The signals are one million times stronger than a satellite navigation signal and the accuracy has been demonstrated to in the order of 18 cm at 30 miles. System accuracy is on the order of 3 nanoseconds, more accurate than the atomic clocks installed on satellites. Another advantage over satellite navigation is that Locata will work inside of buildings. The Locata receive will also track both satellite signals and Locata signals. There are no atomic clocks, no cables and no reference network required for time corrections. Locata can synchronize in only 10 picoseconds.

BAE has recently developed a UAV navigation system that uses signals of opportunity called Navigation Signals of Opportunity (NAVSOP). This system can use cell phone towers, radio towers, microwave towers, TV signals and WiFi signals. All of these types of signals, if available, can be used to triangulate position. This appears to be an excellent solution, however there are some problems. One problem is that if the UAV is very close to a single cell phone tower, it will reject signals from other towers and a triangulation estimate will not be possible.  Another effort is underway to combine the NAVSOP solution with other sensor position estimates. All of these inputs can be combined in a giant Kalman filter to select the best position estimate.

A final alternative navigation system that is in development is a cold atom IMU. Ultra cold atom interferometers are used to measure forces acting on matter. Atoms are cooled to a very low temperature where they can be controlled. An INS that uses this technology will have very low drift rates. A typical INS has a drift rate of 1400 m/hr and a cold atom INS will have drift rates on the order of 5 m/hr. The goal is to demonstrate high precision atom interferometer INS. This system has the potential to replace satellite navigation as a primary solution in any types of air, ground, sea and space platforms.

立体视觉算法--SAD+DP

http://blog.csdn.net/renshengrumenglibing/article/details/8544057

#include <cstdio>
#include <cstring>
#include <iostream>
#include<cv.h>
#include<highgui.h>
#include <cmath>

using namespace std;
const int Width =  512;
const int Height = 512;
int Ddynamic[Width][Width];

int  main()
{
    //打开文件
    FILE* pFile;
    if (pFile = fopen("data.txt","a"))
    {
        //cout <<"File Open Success"<<endl;
    }else{
        //cout <<"File Open Failed"<<end;
        return 0;
    }
    // IplImage * leftImage = cvLoadImage("l1.png",0);
    // IplImage * rightImage = cvLoadImage("r1.png",0);

    //IplImage * leftImage = cvLoadImage("l2.jpg",0);
    //IplImage * rightImage = cvLoadImage("r2.jpg",0);


    IplImage * leftImage = cvLoadImage("left.bmp",0);                        
    IplImage * rightImage = cvLoadImage("right.bmp",0);

    int imageWidth = leftImage->width;
    int imageHeight =leftImage->height;

    IplImage * leftDepth = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
    IplImage * rightDepth = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);

    IplImage * leftValid = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
    IplImage * rightValid = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);

    IplImage * leftFilter = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
    IplImage * rightFilter = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);

    IplImage * depth = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);
    IplImage * valid = cvCreateImage(cvGetSize(leftImage),leftImage->depth,1);

    unsigned char * pPixel = NULL;
    unsigned char  pixel;
    unsigned char * pPixel2 = NULL;
    unsigned char  pixel2;
    cvZero(leftDepth);
    cvZero(rightDepth);
    cvZero(leftValid);
    cvZero(rightValid);
    cvZero(leftFilter);
    cvZero(rightFilter);
    cvZero(depth);
    cvZero(valid);



    CvSize SADWindowSize = cvSize(5,5);  //(widh,height)
    int SW2 = SADWindowSize.width/2;
    int SH2 = SADWindowSize.height/2;
    cout<<"SH2:  "<<SH2<<endl;
    cout<<"SW2:  "<<SW2<<endl;

    int minD = 0;
    int maxD = 15;
    //假设图像是经过矫正的，那么每次都只是需要搜搜同一行的内容
    int max12Diff = 8*SADWindowSize.width*SADWindowSize.height;

    for (int i = 0;i < imageWidth;i++)
    {
        Ddynamic[0][i] = 0;
        Ddynamic[i][0] = 0;
    }
    unsigned char * pLeftPixel  = NULL;
    unsigned char * pRightPixel = NULL;
    unsigned char leftPixel = 0;
    unsigned char rightPixel =0;
    int m,n,l;
    int difPixel = 0;
    int t1 = clock();
    for (int i = SH2 ; i < imageHeight -SH2;i++)
    {
        for (int j = SW2; j<imageWidth - SW2;j++)
        {
            for (int k = j + minD; k <= j + maxD;k++)
            {
                difPixel = 0;
                for (m = -SH2;m <= SH2;m++)
                {
                    for (n = -SW2; n <= SW2;n++)
                    {
                        pRightPixel= (unsigned char*)rightImage->imageData+
                            (i+m)*rightImage->widthStep + j+n;
                        rightPixel = *pRightPixel;
                        if (k < SW2  || k  >= imageWidth -SW2 )
                        {
                            difPixel += rightPixel;
                        }else {
                            pLeftPixel = (unsigned char*)leftImage->imageData +
                                (i+m)*leftImage->widthStep + k+n;
                            leftPixel  = *pLeftPixel;
                            difPixel += abs(leftPixel - rightPixel);
                        }

                    }
                }
                if (difPixel <= max12Diff)
                {
                    Ddynamic[j + 1][k + 1] = Ddynamic[j][k] +1;
                }else if (Ddynamic[j][k+1] > Ddynamic[j+1][k])
                {
                    Ddynamic[j + 1][k + 1] = Ddynamic[j][k+1];
                }else{
                    Ddynamic[j+1][k+1] = Ddynamic[j+1][k];
                }

                //cout<<Ddynamic[j +1][k+1]<<"  ";
            }
            //cout<<"\n";
        }
        //逆向搜索，找出最佳路径
        m = imageWidth - SW2;
        n = imageWidth - SW2;
        int m2 = m, n2 = n;

        while( m >= 1 && n >= 1)
        {
            if ((m2 == m + 1 && n2 >= n +1) || ( m2 > m +1 && n2 == n + 1))
            {
                pPixel = (unsigned char *)rightDepth->imageData + i*rightDepth->widthStep + m;
                *pPixel = (n-m)*10;
                //标记有效匹配点
                pPixel = (unsigned char *)rightValid->imageData + i*rightValid->widthStep + m;
                *pPixel = 255;

                m2 = m;
                n2 = n;
            }
            if (Ddynamic[m-1][n-1] >= Ddynamic[m][n -1] && Ddynamic[m-1][n -1] >= Ddynamic[m-1][n])
            {

                m--;
                n--;
            }else if (Ddynamic[m-1][n] >= Ddynamic[m][n -1] && Ddynamic[m-1][n] >= Ddynamic[m-1][n -1])
            {
                m--;
            }
            else
            {
                n--;
            }

        }

        //cvWaitKey(0);
    }
    int t2 = clock();
    cout<<"dt: "<<t2-t1<<endl;
    //统计未能匹配点的个数
    int count = 0;
    for (int i = 0 ;i< imageHeight;i++)
    {
        for (int j= 0; j< imageWidth;j++)
        {
            pPixel = (unsigned char *)rightValid->imageData + i*rightValid->widthStep + j;
            pixel = *pPixel;
            if (pixel == 0)
            {
                count++;
            }
        }
    }
    cout<<"rightCount:  "<<count<<"  "<<(double)count/(imageWidth*imageHeight)<<endl;
    rightFilter = cvCloneImage(rightDepth);
    //7*7中值滤波
    int halfMedianWindowSize = 3;
    int medianWindowSize = 2*halfMedianWindowSize + 1;
    int medianArray[100] = {0};
    count = 0;
    int temp = 0;
    int medianVal = 0;

    for (int i = halfMedianWindowSize + 1 ;i< imageHeight - halfMedianWindowSize;i++)
    {
        for (int j = halfMedianWindowSize; j< imageWidth - halfMedianWindowSize;j++)
        {
            pPixel = (unsigned char *)rightValid->imageData + i*rightValid->widthStep + j;
            pixel = *pPixel;
            if (pixel == 0)
            {
                count = 0;
                for (int m = i - halfMedianWindowSize ; m <= i + halfMedianWindowSize ;m++)
                {
                    for (int n = j - halfMedianWindowSize; n <= j + halfMedianWindowSize ;n++)
                    {
                        pPixel2 = (unsigned char *)rightDepth->imageData + m*rightDepth->widthStep + n;
                        pixel2 = *pPixel2;
                        if (pixel2 != 0)
                        {
                            medianArray[count] = pixel2;
                            count++;
                        }

                    }
                    //排序
                    for (int k = 0; k< count;k++)
                    {
                        for (int l = k + 1; l< count;l++)
                        {
                            if (medianArray[l] < medianArray[l-1] )
                            {
                                temp = medianArray[l];
                                medianArray[l] = medianArray[l-1];
                                medianArray[l-1] = temp;
                            }
                        }
                    }
                    medianVal = medianArray[count/2];
                    pPixel = (unsigned char *)rightFilter->imageData + i*rightFilter->widthStep + j;
                    *pPixel = medianVal;
                }

            }
        }
    }

    //两次进行DP计算，分别是左对右和右对左,从右想左扫描
    for (int j = 0; j<= imageWidth;j++)
    {
        for (int k = 0;k <= imageWidth;k++)
        {
            Ddynamic[j][k] = 0;
        }
    }
    minD = -maxD;
    maxD = 0;

    for (int i = SH2 ; i < imageHeight -SH2;i++)
    {
        for (int j = imageWidth-SW2-1; j >= SW2;j--)
        {
            for (int k = j + maxD; k >= j + minD;k--)
            {
                difPixel = 0;
                for (m = -SH2;m <= SH2;m++)
                {
                    for (n = -SW2; n <= SW2;n++)
                    {
                        pLeftPixel = (unsigned char*)leftImage->imageData +
                                     (i+m)*leftImage->widthStep + j+n;
                        leftPixel  = *pLeftPixel;
                        if (k < SW2 || k  >= imageWidth -SW2)
                        {
                             difPixel += leftPixel;
                        }else {
                         
                            pRightPixel= (unsigned char*)rightImage->imageData+
                                         (i+m)*rightImage->widthStep + k+n;
                            rightPixel = *pRightPixel;
                            difPixel += abs(leftPixel - rightPixel);
                            //cout<<Ddynamic[j +1][k+1]<<"  ";
                        }
                    }
                }

                if (difPixel <= max12Diff)
                {
                    Ddynamic[j][k] = Ddynamic[j+1][k+1] +1;
                }else if (Ddynamic[j+1][k] > Ddynamic[j][k+1])
                {
                    Ddynamic[j][k] = Ddynamic[j+1][k];
                }else{
                    Ddynamic[j][k] = Ddynamic[j][k+1];
                }

            }
            //cout<<"\n";
        }
        //逆向搜索，找出最佳路径

        m = 0;
        n = 0;
        int m2 = m, n2 = n;
        while( m < imageWidth && n < imageWidth)
        {
            if ((m == m2 + 1 && n >= n2 +1) || ( m > m2 +1 && n == n2 + 1))
            {
                pPixel = (unsigned char *)leftDepth->imageData + i*leftDepth->widthStep + m;
                *pPixel = (m-n)*10;
                //标记有效匹配点
                pPixel = (unsigned char *)leftValid->imageData + i*leftValid->widthStep + m;
                *pPixel = 255;

                m2 = m;
                n2 = n;
            }
            if (Ddynamic[m+1][n+1] >= Ddynamic[m][n+1] && Ddynamic[m+1][n+1] >= Ddynamic[m+1][n])
            {

                m++;
                n++;
            }else if (Ddynamic[m+1][n] >= Ddynamic[m][n +1] && Ddynamic[m+1][n] >= Ddynamic[m+1][n+1])
            {
                m++;
            }
            else
            {
                n++;
            }

        }
        //cvWaitKey(0);
    }
    int t3 = clock();
    //refine the depth image  7*7中值滤波
    //统计未能匹配点的个数
    count = 0;
    for (int i = 0 ;i< imageHeight;i++)
    {
        for (int j= 0; j< imageWidth;j++)
        {
            pPixel = (unsigned char *)leftValid->imageData + i*leftValid->widthStep + j;
            pixel = *pPixel;
            if (pixel == 0)
            {
                count++;
            }
        }
    }

    cout<<"Left Count:  "<<count<<"  "<<(double)count/(imageWidth*imageHeight)<<endl;

    //将关键信息计入文件
    fprintf(pFile,"\nDP:\nClock:   %dms",t3-t1);
    fprintf(pFile,"\nInvalid Point:   %f",(double)count/(imageWidth*imageHeight));
    // cvWaitKey(0);
    leftFilter = cvCloneImage(leftDepth);
    //7*7中值滤波
    count = 0;
    for (int i = halfMedianWindowSize + 1 ;i< imageHeight - halfMedianWindowSize;i++)
    {
        for (int j = halfMedianWindowSize; j< imageWidth - halfMedianWindowSize;j++)
        {
            pPixel = (unsigned char *)leftValid->imageData + i*leftValid->widthStep + j;
            pixel = *pPixel;
            if (pixel == 0)
            {
                count = 0;
                for (int m = i - halfMedianWindowSize ; m <= i + halfMedianWindowSize ;m++)
                {
                    for (int n = j - halfMedianWindowSize; n <= j + halfMedianWindowSize ;n++)
                    {
                        pPixel2 = (unsigned char *)leftDepth->imageData + m*leftDepth->widthStep + n;
                        pixel2 = *pPixel2;
                        if (pixel2 != 0)
                        {
                            medianArray[count] = pixel2;
                            count++;
                        }

                    }
                    //排序
                    for (int k = 0; k< count;k++)
                    {
                        for (int l = k + 1; l< count;l++)
                        {
                            if (medianArray[l] < medianArray[l-1] )
                            {
                                temp = medianArray[l];
                                medianArray[l] = medianArray[l-1];
                                medianArray[l-1] = temp;
                            }
                        }
                    }
                    medianVal = medianArray[count/2];
                    pPixel = (unsigned char *)leftFilter->imageData + i*leftFilter->widthStep + j;
                    *pPixel = medianVal;
                }

            }
        }
    }
    //假如已知左右图片的深度，进行refinement
    unsigned char dLeft = 0, dRight = 0;
    count = 0;
    for (int i = 0; i< imageHeight;i++)
    {
        for (int j = 0; j< imageWidth;j++)
        {
            pRightPixel= (unsigned char*)rightDepth->imageData + i*rightDepth->widthStep + j;
            dRight = (*pRightPixel)/10;
            if (j + dRight < imageWidth)
            {  
                pPixel= (unsigned char*)depth->imageData + i*depth->widthStep + j;
                pLeftPixel= (unsigned char*)leftDepth->imageData + i*leftDepth->widthStep + j + dRight;
                dLeft = (*pLeftPixel)/10;
                if (abs(int(dRight) -(int)dLeft) <= 1)
                {
                    //深度一致
                    *pPixel = dRight*10;
                 
                }else{
                    //深度不一致，认为是误匹配、错误匹配
                    *pPixel = 0;
                    count++;
                }
            }
        }
    }
    cout<<"depth Count:  "<<count<<"  "<<(double)count/(imageWidth*imageHeight)<<endl;


    cvNamedWindow("leftImage",1);
    cvNamedWindow("rightImage",1);
    cvNamedWindow("leftDepth",1);
    cvNamedWindow("leftValid",1);
    cvNamedWindow("rightDepth",1);
    cvNamedWindow("rightValid",1);
    cvNamedWindow("leftFilter",1);
    cvNamedWindow("rightFilter",1);
    cvNamedWindow("depth",1);

 
    cvShowImage("leftImage",leftImage);
    cvShowImage("rightImage",rightImage);

    cvShowImage("leftDepth",leftDepth);
    cvShowImage("rightDepth",rightDepth);

    cvShowImage("leftValid",leftValid);
    cvShowImage("rightValid",rightValid);

    cvShowImage("leftFilter",leftFilter);
    cvShowImage("rightFilter",rightFilter);
    cvShowImage("depth",depth);

    cvSaveImage("leftDepth.jpg",leftDepth);
    cvSaveImage("rightDepth.jpg",rightDepth);
    cvSaveImage("leftValid.jpg",leftValid);
    cvSaveImage("rightValid.jpg",rightValid);
    cvSaveImage("leftFilter.jpg",leftFilter);
    cvSaveImage("rightFilter.jpg",rightFilter);
    cvSaveImage("depth.jpg",depth);



    fclose(pFile);
    cvWaitKey(0);
    return 0;
}

6DOF motion equation

6DOF equation (Euler Angles)

from http://www.mathworks.com/help/aeroblks/6dofeulerangles.html

The 6DoF (Euler Angles) block considers the rotation of a body-fixed coordinate frame (X_b , Y_b , Z_b ) about a flat Earth reference frame (X_e , Y_e , Z_e ). The origin of the body-fixed coordinate frame is the center of gravity of the body, and the body is assumed to be rigid, an assumption that eliminates the need to consider the forces acting between individual elements of mass. The flat Earth reference frame is considered inertial, an excellent approximation that allows the forces due to the Earth's motion relative to the "fixed stars" to be neglected.

The translational motion of the body-fixed coordinate frame is given below, where the applied forces [F_x F_yF_z ]^T are in the body-fixed frame, and the mass of the body m is assumed constant.

        

The rotational dynamics of the body-fixed frame are given below, where the applied moments are [L M N ]^T, and the inertia tensor I is with respect to the origin O.

The relationship between the body-fixed angular velocity vector, [p q r]^T, and the rate of change of the Euler angles, , can be determined by resolving the Euler rates into the body-fixed coordinate frame

 

Inverting J then gives the required relationship to determine the Euler rate vector.

背景知识

     The cross product is anticommutative,

     

The vector cross product also can be expressed as the product of a skew-symmetric matrix and a vector:^[9]

where superscript ^T refers to the transpose operation, and [a]_× is defined by:

Animation in Matlab

http://www.maelabs.ucsd.edu/mae150/mae150_resources/Matlab_Tips/intro_to_kinematic_plotting.htm

%--------------- Introduction to Kinematic Plotting ---------------

%

% MATLAB provides three ways of generating animations:

% 1. Repeatedly execute the 'plot' command in a loop. This is the easiest method

%    but results in a flashing screen.  (see the first example)

%

% 2. Set up a graph using handle graphics, and repeatedly execute the 'drawnow' command.

%    This eliminates the flashing problem. (see the second example)

% 3. Save a number of different pictures and then play them back as a movie. This is useful for very complex figures that would be slow to animate using the prior methods. (see Matlab help)

% Simple (but effective) visualizations can be done with line plots. That is, draw a new line in a new location after a short pause and erase the old line.

% (to plot 3D shapes see the patch command)

% Matlab can help visualize and animate the kinematics of mechanisms and other objects

As you can see, this code allows you to create a simple animation.

%------------ A simple animation using 'plot' and 'drawnow' ------------

% A Simple Animation of a rotation arm

theta_step = 5*pi/180;

r = 1;               % radius of the arm

for theta=0:theta_step:2*pi

% In order to draw a stick figure you need to have a start point and an end point

% (in the case of a closed figure the end point must end at the start point;

% this allows the figure to be completely closed), the following two commands define

% the points of the stick figure:

    data_pt(1,:) = [0 0];                        % first point is the origin

    data_pt(2,:) = [r*cos(theta) r*sin(theta)];  % second point is end of vector

    plot(data_pt(:,1),data_pt(:,2));             % connect the points with a line

    axis([-1.5 1.5 -1.5 1.5]);   % set the axis limit to include the range of animation

    axis manual     % freezes the axis scaling

    pause(0.1)      % puts a delay in seconds (if time is zero, a keyboard hit is required)

end

disp('Hit the Space Bar to Continue');

pause;

%********************* Ways to reduce flashing ***********************

%  Do the animation again while reduce flashing.

%  There are two steps to reduce the flashing on the plot. Please see

%  ‘Introduction to Graphic Handles’ for help on graphic handles.

%  1. define EraseMode in the ‘plot’ command. 

%  2. instead of using ‘plot’ repeatedly, change the data by ‘set’ command.

theta_step = 5*pi/180;

theta = 0:theta_step:2*pi;          % generate and store the angle sequence

x_pt = [0 1];                       % define the end points of the stick figure

y_pt=[0 0];             

axis([-1.5 1.5 -1.5 1.5]);

hold on;

hndl=plot(x_pt,y_pt,'EraseMode','background'); % connect the end points,

                                               % and use "EraseMode" to REDUCE FLASHING

set(gca,'DataAspectRatio',[1 1 1]);            % make x,y-axis relative scaling equal

for j=1:length(theta)

    x_pt=[0  cos(theta(j))];

    y_pt=[0  sin(theta(j))];

    set(hndl,'XData',x_pt,'YData',y_pt);       % REDUCE FLASHING

    drawnow         % forces Matlab to redraw the plot at the current values

    pause(.1)

end

%  Compare this animation with the previous one: 

%  In this case, we generate and store the end points BEFORE we call them in each loop.

%  In the previous one, we generate the end points and draw a new line IN each loop.

%  When the process of generating data is complex and the computational time delay

%  cannot be ignored, we usually do the computation first, then draw the data in each loop

%------------------------------------------------------------

% Key functions:  drawnow, pause(time), set

% Key properties: EraseMode, DataAspectRatio, XData, YData