#include <opencv2/imgproc/imgproc.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <opencv2/core/core.hpp>

#include <opencv2/features2d/features2d.hpp>

#include <opencv2/nonfree/nonfree.hpp>

#include "ros/ros.h"                //ROS HEADER

using namespace cv;

using namespace std;

int main (void){

        Mat Dp(3,3,CV_32FC1);

        Mat Dc(3,3,CV_32FC1);

        Mat Q(3,3,CV_32FC1);

        Mat U,Vt,R,T,W;

        Dp.at<float>(0,0)=0.86483884; Dp.at<float>(0,1)= -0.3077251; Dp.at<float>(0,2)=-0.55711365;

        Dp.at<float>(1,0)=0.49294353; Dp.at<float>(1,1)=-0.24209651; Dp.at<float>(1,2)=-0.25084701;

        Dp.at<float>(2,0)=0;          Dp.at<float>(2,1)=0;           Dp.at<float>(2,2)=0;

        Dc.at<float>(0,0)=0.75632739; Dc.at<float>(0,1)= -0.38859656; Dc.at<float>(0,2)=-0.36773083;

        Dc.at<float>(1,0)=0.9699229; Dc.at<float>(1,1)=-0.49858192; Dc.at<float>(1,2)=-0.47134098;

        Dc.at<float>(2,0)=0.10566688; Dc.at<float>(2,1)=-0.060333252; Dc.at<float>(2,2)=-0.045333147;

        Q=Dp*Dc.t();

        SVD decomp;

        decomp=SVD(Q);

        U=decomp.u;

        Vt=decomp.vt;

        W=decomp.w;

        R=Vt.t()*U.t();

        cout << "Dp=" << endl << Dp << endl;

              cout << "Dc=" << endl << Dc << endl;

            cout << "Q=" << endl << Q << endl;

              cout << "U=" << endl << U << endl;

        cout << "W=" << endl << W << endl;

              cout << "Vt=" << endl << Vt << endl;

        return(0);

};