Commits

Suyang Dong committed 274aa84

add cv2cg folder

Comments (0)

Files changed (33)

Aurora/BuildingDamage.h

 \************************************************************************/
 #ifndef ARQUAKE_BUILDING_DAMAGE_INCLUDE
 #define ARQUAKE_BUILDING_DAMAGE_INCLUDE
-#include "direct.h"
+
 #include <osg/Camera>
 #include <osg/Matrix>
 #include <iostream>
 #include <string>
-#include "Log.h"
-#include "OpenCVHelper.h"
-#include "CV2CG.h"
+#include "../third_party/cv2cg/CV2CG.h"
 #include "BuildingModel.h"
 #define USE_OPIMIZATION	// uncomment this if you want NO 1st and 2nd criteria opimization
 #define USE_UNIFIED_SHIFT // assume there is only one X, Y shift in one floor

Aurora/CV2CG.h

-#pragma once
-/*
- *  Copyright (c) 2010  Chen Feng (cforrest (at) umich.edu)
- *    and the University of Michigan
- *
- *  This program 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 2 of the License, or
- *  (at your option) any later version.
- *
- *  This program 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.
- *
- */
-
-/* CV2CG.h */
-
-//standard include
-#include <iostream>
-#include <iomanip>
-#include <fstream>
-#include <sstream>
-#include <string>
-#include <vector>
-#include <stdio.h>
-#include <time.h>
-
-#include "Log.h"
-#include "OpenCVHelper.h"
-
-//Statement:
-//All notation such as K,R,C,T,P are in standard computer vision manner
-//details in <<Multiple View Geometry>> by Richard Hartley and A. Zisserman
-
-//some explanations of formulas in this file could be found on my undergraduate
-//thesis (in Chinese):
-//http://www-personal.umich.edu/~cforrest/upload/ChenFeng.UnderGrad.Thesis.ch.pdf
-
-namespace CV2CG
-{
-//get CV matrices from an osg::Camera
-//need to provided image size imgWximgH since K depends on them
-inline void cg2cv(const osg::Camera &camera,
-                  double imgW, double imgH,
-                  double K[3][3], double C[3], double R[3][3])
-{
-	double l,r,b,t,n,f;
-	camera.getProjectionMatrixAsFrustum(l,r,b,t,n,f);
-
-	//K
-	//[ alphaX    0     u0 ]
-	//[   0     alphaY  v0 ]
-	//[   0       0      1 ]
-	helper::zeros(3,3,K[0]);
-	K[0][0] = n*imgW/(r-l); //alphaX
-	K[1][1] = n*imgH/(t-b); //alphaY
-	K[0][2] = -imgW*(r+l)/((r-l)*2)+imgW/2;//u0
-	K[1][2] = imgH*(t+b)/((t-b)*2)+imgH/2;//v0
-	K[2][2] = 1;
-
-	osg::Vec3 center,eye,up;
-	camera.getViewMatrixAsLookAt(eye, center, up);
-	C[0]=eye.x();
-	C[1]=eye.y();
-	C[2]=eye.z();
-
-	osg::Matrix vmat = camera.getViewMatrix();
-	R[0][0] =  vmat(0,0);
-	R[0][1] =  vmat(1,0);
-	R[0][2] =  vmat(2,0);
-	R[1][0] = -vmat(0,1);
-	R[1][1] = -vmat(1,1);
-	R[1][2] = -vmat(2,1);
-	R[2][0] = -vmat(0,2);
-	R[2][1] = -vmat(1,2);
-	R[2][2] = -vmat(2,2);
-}
-
-//set up projection matrix of osg::Camera from CV calibration matrix
-inline void cv2cg(const double K[3][3], const double n, const double f,
-                  const double imgW, const double imgH, osg::Camera &camera)
-{
-	//intrinsic
-	double ax=K[0][0]/K[2][2], ay=K[1][1]/K[2][2],
-	       u0=K[0][2]/K[2][2], v0=K[1][2]/K[2][2]; //insure that K[2][2]==1
-	double l,r,t,b;
-
-	r = ( imgW - u0 ) * n / ax;
-	l = r - n*imgW/ax;
-	t = v0 * n / ay;
-	b = t - n*imgH/ay;
-
-	camera.setProjectionMatrixAsFrustum(l,r,b,t,n,f);
-}
-
-//set up modelview matrix of osg::Camera from CV calibration matrix
-//!!! note the difference of T and C here!!!
-inline void cv2cg(const double T[3], const double R[3][3], osg::Camera &camera,
-                  bool TisC=false)
-{
-	if(TisC) {
-		double tT[3];
-		helper::mul(3,3,3,1,R[0],T,tT);
-		tT[0]*=-1;
-		tT[1]*=-1;
-		tT[2]*=-1;
-		//extrinsic
-		osg::Matrix vmat(
-		    R[0][0], -R[1][0], -R[2][0], 0,
-		    R[0][1], -R[1][1], -R[2][1], 0,
-		    R[0][2], -R[1][2], -R[2][2], 0,
-		    tT[0], -tT[1], -tT[2], 1            //ATTENTION! should set as (T0,-T1,-T2)
-		);
-		camera.setViewMatrix(vmat);
-	} else {
-		//extrinsic
-		osg::Matrix vmat(
-		    R[0][0], -R[1][0], -R[2][0], 0,
-		    R[0][1], -R[1][1], -R[2][1], 0,
-		    R[0][2], -R[1][2], -R[2][2], 0,
-		    T[0], -T[1], -T[2], 1            //ATTENTION! should set as (T0,-T1,-T2)
-		);
-		camera.setViewMatrix(vmat);
-	}
-}
-
-
-//setup an osg::Camera from CV matrices
-inline void cv2cg(const double K[3][3], const double C[3],
-                  const double R[3][3], const double n, const double f,
-                  const double imgW, const double imgH, osg::Camera &camera)
-{
-	//intrinsic
-	double ax=K[0][0]/K[2][2], ay=K[1][1]/K[2][2],
-	       u0=K[0][2]/K[2][2], v0=K[1][2]/K[2][2]; //insure that K[2][2]==1
-	double l,r,t,b;
-
-	r = ( imgW - u0 ) * n / ax;
-	l = r - n*imgW/ax;
-	t = v0 * n / ay;
-	b = t - n*imgH/ay;
-
-	camera.setProjectionMatrixAsFrustum(l,r,b,t,n,f);
-
-	//extrinsic
-	double T[3];
-	helper::mul(3,3,3,1,R[0],C,T);
-	T[0]*=-1;
-	T[1]*=-1;
-	T[2]*=-1;
-	osg::Matrix vmat(
-	    R[0][0], -R[1][0], -R[2][0], 0,
-	    R[0][1], -R[1][1], -R[2][1], 0,
-	    R[0][2], -R[1][2], -R[2][2], 0,
-	    T[0], -T[1], -T[2], 1            //ATTENTION! should set as (T0,-T1,-T2)
-	);
-	camera.setViewMatrix(vmat);
-}
-
-//this actually use a osg camera to get cv photo coordinate
-inline void CG_Project(
-    const osg::Camera &camera, //camera
-    double x, double y, double z, //world 3d point
-    double &u, double &v //image 2d point
-)
-{
-	osg::Vec3 wp(x,y,z);
-	osg::Vec3 ep = wp * camera.getViewMatrix();
-	double l,r,b,t,n,f;
-	camera.getProjectionMatrixAsFrustum(l,r,b,t,n,f);
-	const osg::Image *img = dynamic_cast<const osg::Image *>(
-	                            camera.getUserData());
-	double imgW=500, imgH=500;
-	if(!img) {
-		const osg::Viewport *vp = camera.getViewport();
-		if(vp) {
-			imgW = vp->width();
-		}
-		imgH = vp->height();
-	} else {
-		imgW=img->s(), imgH=img->t();
-	}
-	//const osg::Viewport* vp = camera.getViewport();
-	//double x0 = vp?vp->x():0, y0 = vp?vp->y():0,
-	//	w = vp?:vp->width():1, h = vp?vp->height():1;
-	double xe=ep.x(), ye=ep.y(), ze=ep.z();
-	//u = -n*w*xe/((r-l)*ze)-w*(r+l)/((r-l)*2)+x0+w/2;
-	//v = -n*h*ye/((t-b)*ze)-h*(t+b)/((t-b)*2)+y0+h/2;
-	u = -n*imgW*xe/((r-l)*ze)-imgW*(r+l)/((r-l)*2)+imgW/2;
-	v = -n*imgH*ye/((t-b)*ze)-imgH*(t+b)/((t-b)*2)+imgH/2;
-	// be careful, this change the photo coordinate system to computer vision
-	v = imgH-v;
-}
-
-inline std::ostream &operator<<(std::ostream &os, const osg::Matrix &mat)
-{
-	os << std::setiosflags(std::ios::scientific);
-	for(int i=0; i<4; ++i) {
-		for(int j=0; j<4; ++j) {
-			os << mat(i,j) << "\t";
-		}
-		os << std::endl;
-	}
-	return os;
-}
-
-inline std::ostream &operator<<(std::ostream &os, const osg::Camera &camera)
-{
-	os << std::setiosflags(std::ios::scientific);
-	osg::Matrix vmat = camera.getViewMatrix();
-	osg::Matrix pmat = camera.getProjectionMatrix();
-	os << "View Matrix = " << std::endl;
-	os << vmat ;
-	os << "Projection Matrix = " << std::endl;
-	os << pmat ;
-	double l,r,b,t,n,f;
-	camera.getProjectionMatrixAsFrustum(l,r,b,t,n,f);
-	os <<
-	   "l=" << l << " r=" << r <<
-	   " b=" << b << " t=" << t <<
-	   " n=" << n << " f=" << f <<std::endl;
-	const osg::Viewport &viewport = *(camera.getViewport());
-	os << "x=" << viewport.x() << " y=" << viewport.y()
-	   << " w=" << viewport.width() << " h=" << viewport.height() << std::endl;
-	return os;
-}
-
-inline void CG_Report(const osg::Camera &camera,
-                      std::ostream &out=std::cout)
-{
-	out<<"#------------Graphics--------------"<<std::endl;
-	out<<camera<<std::endl;
-	out<<"#Test CG Projection:"<<std::endl;
-	double u,v;
-	CG_Project(camera, 0, 0, 0, u, v);
-	out<<"#[0,0,0]->["<<u<<","<<v<<"]"<<std::endl;
-}
-
-inline void CV_Report(const osg::Camera &camera,
-                      std::ostream &out=std::cout)
-{
-	const osg::Image *img = dynamic_cast<const osg::Image *>(
-	                            camera.getUserData());
-	double imgW=500, imgH=500;
-	if(!img) {
-		out << "#Camera do not have valid Image as UserData!"
-		    " use viewport size instead!"<<std::endl;
-		const osg::Viewport *vp = camera.getViewport();
-		if(!vp) {
-			out << "#No valid viewport, use default image size!"<<std::endl;
-		} else {
-			imgW = vp->width();
-		}
-		imgH = vp->height();
-		out << "imgW=" << imgW << " imgH=" <<imgH<<std::endl;
-	} else {
-		imgW=img->s(), imgH=img->t();
-		out << "imgW=" << imgW << " imgH=" <<imgH<<std::endl;
-	}
-
-	out<<"#------------Vision----------------"<<std::endl;
-	double K[3][3],C[3],T[3],R[3][3],P[3][4];
-	cg2cv(camera, imgW, imgH, K,C,R);
-	helper::compose(K[0],R[0],C,P[0],true);
-	helper::mul(3,3,3,1,R[0],C,T);
-	T[0]*=-1;
-	T[1]*=-1;
-	T[2]*=-1;
-
-	out<<"K(alphaX alphaY u0 v0)="<<std::endl,
-	   out<<K[0][0]<<"\n"<<K[1][1]<<"\n"<<K[0][2]<<"\n"<<K[1][2]<<std::endl;
-	out<<"R=\n"<<helper::PrintMat<>(3,3,R[0]);
-	out<<"C=\n"<<helper::PrintMat<>(3,1,C);
-	out<<"T=\n"<<helper::PrintMat<>(3,1,T);
-	out<<"P=\n"<<helper::PrintMat<>(3,4,P[0]);
-	double nu,nv;
-	helper::project(P[0],0,0,0,nu,nv);
-	out<<"#Test CV Projection:"<<std::endl;
-	out<<"#[0,0,0]->["<<nu<<","<<nv<<"]"<<std::endl;
-}
-
-inline void CV_CG_Report(const osg::Camera &camera,
-                         std::ostream &out=std::cout)
-{
-	CG_Report(camera, out);
-	CV_Report(camera, out);
-}
-
-inline void CG_Project_Report(const osg::Camera &camera,
-                              const osg::Vec3Array *v3a, std::ostream &out=std::cout)
-{
-	out << std::setiosflags(std::ios::scientific);
-	for(unsigned int i=0; i<v3a->size(); ++i) {
-		const osg::Vec3 &v3 = v3a->at(i);
-		double pu,pv;
-		CG_Project(camera,v3.x(),v3.y(),v3.z(),pu,pv);
-		out << v3.x() << " " << v3.y() << " " << v3.z() << " "
-		    << pu << " " << pv << std::endl;
-	}
-}
-
-}//end of namespace CV2CG

Aurora/FirstPersonViewHandler.cpp

 \*********************************************************************************************************/
 #include "FirstPersonViewHandler.h"
 #include "ViewOrganizer.h"
-#include "Log.h"
-#include "OpenCVHelper.h"
-#include "CV2CG.h"
 #include <osg/Quat>
 #include <osg/Vec3d>
 #include <osg/CopyOp>

Aurora/PhotoViewHandler.cpp

 #include <osgViewer/View>
 #include <osg/Geometry>
 #include <osg/LineWidth>
-#include "../third_party/lsd.h"
 #include <osg/Math>
 extern bool gShaking;
 extern std::string getDataPath();

Aurora/PhotoViewHandler.h

 #include <osg/Geometry>
 #include <osg/Vec4d>
 #include <vector>
-#include "OpenCVHelper.h"
-#include "CV2CG.h"
 #include "mathHelper.h"
 #include "driftEstimate.h"
 using namespace std;

Aurora/ViewOrganizer.cpp

Binary file modified.

Aurora/mathHelper.h

 #include <osg/Vec4d>
 #include <osg/Camera>
 #include "../third_party/lsd.h"
-#include "CV2CG.h"
+#include "../third_party/cv2cg/CV2CG.h"
 
 #define PITCH_ERROR
 #define YAW_ERROR

third_party/cv2cg/CV2CG.h

+#pragma once
+/*
+ *  Copyright (c) 2010  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* CV2CG.h */
+
+//standard include
+#include <iostream>
+#include <iomanip>
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <stdio.h>
+#include <time.h>
+
+#include "Log.h"
+#include "OpenCVHelper.h"
+
+//Statement:
+//All notation such as K,R,C,T,P are in standard computer vision manner
+//details in <<Multiple View Geometry>> by Richard Hartley and A. Zisserman
+
+//some explanations of formulas in this file could be found on my undergraduate
+//thesis (in Chinese):
+//http://www-personal.umich.edu/~cforrest/upload/ChenFeng.UnderGrad.Thesis.ch.pdf
+
+namespace CV2CG
+{
+//get CV matrices from an osg::Camera
+//need to provided image size imgWximgH since K depends on them
+inline void cg2cv(const osg::Camera &camera,
+                  double imgW, double imgH,
+                  double K[3][3], double C[3], double R[3][3])
+{
+	double l,r,b,t,n,f;
+	camera.getProjectionMatrixAsFrustum(l,r,b,t,n,f);
+
+	//K
+	//[ alphaX    0     u0 ]
+	//[   0     alphaY  v0 ]
+	//[   0       0      1 ]
+	helper::zeros(3,3,K[0]);
+	K[0][0] = n*imgW/(r-l); //alphaX
+	K[1][1] = n*imgH/(t-b); //alphaY
+	K[0][2] = -imgW*(r+l)/((r-l)*2)+imgW/2;//u0
+	K[1][2] = imgH*(t+b)/((t-b)*2)+imgH/2;//v0
+	K[2][2] = 1;
+
+	osg::Vec3 center,eye,up;
+	camera.getViewMatrixAsLookAt(eye, center, up);
+	C[0]=eye.x();
+	C[1]=eye.y();
+	C[2]=eye.z();
+
+	osg::Matrix vmat = camera.getViewMatrix();
+	R[0][0] =  vmat(0,0);
+	R[0][1] =  vmat(1,0);
+	R[0][2] =  vmat(2,0);
+	R[1][0] = -vmat(0,1);
+	R[1][1] = -vmat(1,1);
+	R[1][2] = -vmat(2,1);
+	R[2][0] = -vmat(0,2);
+	R[2][1] = -vmat(1,2);
+	R[2][2] = -vmat(2,2);
+}
+
+//set up projection matrix of osg::Camera from CV calibration matrix
+inline void cv2cg(const double K[3][3], const double n, const double f,
+                  const double imgW, const double imgH, osg::Camera &camera)
+{
+	//intrinsic
+	double ax=K[0][0]/K[2][2], ay=K[1][1]/K[2][2],
+	       u0=K[0][2]/K[2][2], v0=K[1][2]/K[2][2]; //insure that K[2][2]==1
+	double l,r,t,b;
+
+	r = ( imgW - u0 ) * n / ax;
+	l = r - n*imgW/ax;
+	t = v0 * n / ay;
+	b = t - n*imgH/ay;
+
+	camera.setProjectionMatrixAsFrustum(l,r,b,t,n,f);
+}
+
+//set up modelview matrix of osg::Camera from CV calibration matrix
+//!!! note the difference of T and C here!!!
+inline void cv2cg(const double T[3], const double R[3][3], osg::Camera &camera,
+                  bool TisC=false)
+{
+	if(TisC) {
+		double tT[3];
+		helper::mul(3,3,3,1,R[0],T,tT);
+		tT[0]*=-1;
+		tT[1]*=-1;
+		tT[2]*=-1;
+		//extrinsic
+		osg::Matrix vmat(
+		    R[0][0], -R[1][0], -R[2][0], 0,
+		    R[0][1], -R[1][1], -R[2][1], 0,
+		    R[0][2], -R[1][2], -R[2][2], 0,
+		    tT[0], -tT[1], -tT[2], 1            //ATTENTION! should set as (T0,-T1,-T2)
+		);
+		camera.setViewMatrix(vmat);
+	} else {
+		//extrinsic
+		osg::Matrix vmat(
+		    R[0][0], -R[1][0], -R[2][0], 0,
+		    R[0][1], -R[1][1], -R[2][1], 0,
+		    R[0][2], -R[1][2], -R[2][2], 0,
+		    T[0], -T[1], -T[2], 1            //ATTENTION! should set as (T0,-T1,-T2)
+		);
+		camera.setViewMatrix(vmat);
+	}
+}
+
+
+//setup an osg::Camera from CV matrices
+inline void cv2cg(const double K[3][3], const double C[3],
+                  const double R[3][3], const double n, const double f,
+                  const double imgW, const double imgH, osg::Camera &camera)
+{
+	//intrinsic
+	double ax=K[0][0]/K[2][2], ay=K[1][1]/K[2][2],
+	       u0=K[0][2]/K[2][2], v0=K[1][2]/K[2][2]; //insure that K[2][2]==1
+	double l,r,t,b;
+
+	r = ( imgW - u0 ) * n / ax;
+	l = r - n*imgW/ax;
+	t = v0 * n / ay;
+	b = t - n*imgH/ay;
+
+	camera.setProjectionMatrixAsFrustum(l,r,b,t,n,f);
+
+	//extrinsic
+	double T[3];
+	helper::mul(3,3,3,1,R[0],C,T);
+	T[0]*=-1;
+	T[1]*=-1;
+	T[2]*=-1;
+	osg::Matrix vmat(
+	    R[0][0], -R[1][0], -R[2][0], 0,
+	    R[0][1], -R[1][1], -R[2][1], 0,
+	    R[0][2], -R[1][2], -R[2][2], 0,
+	    T[0], -T[1], -T[2], 1            //ATTENTION! should set as (T0,-T1,-T2)
+	);
+	camera.setViewMatrix(vmat);
+}
+
+//this actually use a osg camera to get cv photo coordinate
+inline void CG_Project(
+    const osg::Camera &camera, //camera
+    double x, double y, double z, //world 3d point
+    double &u, double &v //image 2d point
+)
+{
+	osg::Vec3 wp(x,y,z);
+	osg::Vec3 ep = wp * camera.getViewMatrix();
+	double l,r,b,t,n,f;
+	camera.getProjectionMatrixAsFrustum(l,r,b,t,n,f);
+	const osg::Image *img = dynamic_cast<const osg::Image *>(
+	                            camera.getUserData());
+	double imgW=500, imgH=500;
+	if(!img) {
+		const osg::Viewport *vp = camera.getViewport();
+		if(vp) {
+			imgW = vp->width();
+		}
+		imgH = vp->height();
+	} else {
+		imgW=img->s(), imgH=img->t();
+	}
+	//const osg::Viewport* vp = camera.getViewport();
+	//double x0 = vp?vp->x():0, y0 = vp?vp->y():0,
+	//	w = vp?:vp->width():1, h = vp?vp->height():1;
+	double xe=ep.x(), ye=ep.y(), ze=ep.z();
+	//u = -n*w*xe/((r-l)*ze)-w*(r+l)/((r-l)*2)+x0+w/2;
+	//v = -n*h*ye/((t-b)*ze)-h*(t+b)/((t-b)*2)+y0+h/2;
+	u = -n*imgW*xe/((r-l)*ze)-imgW*(r+l)/((r-l)*2)+imgW/2;
+	v = -n*imgH*ye/((t-b)*ze)-imgH*(t+b)/((t-b)*2)+imgH/2;
+	// be careful, this change the photo coordinate system to computer vision
+	v = imgH-v;
+}
+
+inline std::ostream &operator<<(std::ostream &os, const osg::Matrix &mat)
+{
+	os << std::setiosflags(std::ios::scientific);
+	for(int i=0; i<4; ++i) {
+		for(int j=0; j<4; ++j) {
+			os << mat(i,j) << "\t";
+		}
+		os << std::endl;
+	}
+	return os;
+}
+
+inline std::ostream &operator<<(std::ostream &os, const osg::Camera &camera)
+{
+	os << std::setiosflags(std::ios::scientific);
+	osg::Matrix vmat = camera.getViewMatrix();
+	osg::Matrix pmat = camera.getProjectionMatrix();
+	os << "View Matrix = " << std::endl;
+	os << vmat ;
+	os << "Projection Matrix = " << std::endl;
+	os << pmat ;
+	double l,r,b,t,n,f;
+	camera.getProjectionMatrixAsFrustum(l,r,b,t,n,f);
+	os <<
+	   "l=" << l << " r=" << r <<
+	   " b=" << b << " t=" << t <<
+	   " n=" << n << " f=" << f <<std::endl;
+	const osg::Viewport &viewport = *(camera.getViewport());
+	os << "x=" << viewport.x() << " y=" << viewport.y()
+	   << " w=" << viewport.width() << " h=" << viewport.height() << std::endl;
+	return os;
+}
+
+inline void CG_Report(const osg::Camera &camera,
+                      std::ostream &out=std::cout)
+{
+	out<<"#------------Graphics--------------"<<std::endl;
+	out<<camera<<std::endl;
+	out<<"#Test CG Projection:"<<std::endl;
+	double u,v;
+	CG_Project(camera, 0, 0, 0, u, v);
+	out<<"#[0,0,0]->["<<u<<","<<v<<"]"<<std::endl;
+}
+
+inline void CV_Report(const osg::Camera &camera,
+                      std::ostream &out=std::cout)
+{
+	const osg::Image *img = dynamic_cast<const osg::Image *>(
+	                            camera.getUserData());
+	double imgW=500, imgH=500;
+	if(!img) {
+		out << "#Camera do not have valid Image as UserData!"
+		    " use viewport size instead!"<<std::endl;
+		const osg::Viewport *vp = camera.getViewport();
+		if(!vp) {
+			out << "#No valid viewport, use default image size!"<<std::endl;
+		} else {
+			imgW = vp->width();
+		}
+		imgH = vp->height();
+		out << "imgW=" << imgW << " imgH=" <<imgH<<std::endl;
+	} else {
+		imgW=img->s(), imgH=img->t();
+		out << "imgW=" << imgW << " imgH=" <<imgH<<std::endl;
+	}
+
+	out<<"#------------Vision----------------"<<std::endl;
+	double K[3][3],C[3],T[3],R[3][3],P[3][4];
+	cg2cv(camera, imgW, imgH, K,C,R);
+	helper::compose(K[0],R[0],C,P[0],true);
+	helper::mul(3,3,3,1,R[0],C,T);
+	T[0]*=-1;
+	T[1]*=-1;
+	T[2]*=-1;
+
+	out<<"K(alphaX alphaY u0 v0)="<<std::endl,
+	   out<<K[0][0]<<"\n"<<K[1][1]<<"\n"<<K[0][2]<<"\n"<<K[1][2]<<std::endl;
+	out<<"R=\n"<<helper::PrintMat<>(3,3,R[0]);
+	out<<"C=\n"<<helper::PrintMat<>(3,1,C);
+	out<<"T=\n"<<helper::PrintMat<>(3,1,T);
+	out<<"P=\n"<<helper::PrintMat<>(3,4,P[0]);
+	double nu,nv;
+	helper::project(P[0],0,0,0,nu,nv);
+	out<<"#Test CV Projection:"<<std::endl;
+	out<<"#[0,0,0]->["<<nu<<","<<nv<<"]"<<std::endl;
+}
+
+inline void CV_CG_Report(const osg::Camera &camera,
+                         std::ostream &out=std::cout)
+{
+	CG_Report(camera, out);
+	CV_Report(camera, out);
+}
+
+inline void CG_Project_Report(const osg::Camera &camera,
+                              const osg::Vec3Array *v3a, std::ostream &out=std::cout)
+{
+	out << std::setiosflags(std::ios::scientific);
+	for(unsigned int i=0; i<v3a->size(); ++i) {
+		const osg::Vec3 &v3 = v3a->at(i);
+		double pu,pv;
+		CG_Project(camera,v3.x(),v3.y(),v3.z(),pu,pv);
+		out << v3.x() << " " << v3.y() << " " << v3.z() << " "
+		    << pu << " " << pv << std::endl;
+	}
+}
+
+}//end of namespace CV2CG

third_party/cv2cg/CameraHelper.h

+#ifndef CAMERAHELPER_H
+#define CAMERAHELPER_H
+/*
+ *  Copyright (c) 2010  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* CameraHelper.h
+   Multiple View Geometry related helper functions */
+
+//standard include
+#include <iostream>
+#include <iomanip>
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <stdio.h>
+#include <time.h>
+//opencv include
+#include "OpenCVHelper.h"
+
+namespace CameraHelper
+{
+
+// s[u,v,1]' = P * [x,y,z,1]'
+// P<3x4>, projection matrix
+inline void project(double const *P,
+                    double x, double y, double z,
+                    double &u, double &v)
+{
+	double w;
+	u = P[IDX(0,0,4)] * x + P[IDX(0,1,4)] * y
+	    + P[IDX(0,2,4)] * z + P[IDX(0,3,4)] * 1;
+	v = P[IDX(1,0,4)] * x + P[IDX(1,1,4)] * y
+	    + P[IDX(1,2,4)] * z + P[IDX(1,3,4)] * 1;
+	w = P[IDX(2,0,4)] * x + P[IDX(2,1,4)] * y
+	    + P[IDX(2,2,4)] * z + P[IDX(2,3,4)] * 1;
+	if(fabs(w)<1e-10) {
+		u = v = 0;    // ideal point
+	} else {
+		u/=w;
+		v/=w;
+	}
+}
+
+inline void project(double const *P,
+                    double const *X, double *p)
+{
+	project(P, X[0], X[1], X[2], p[0], p[1]);
+}
+
+// K<3x3> * [R<3x3> , T<3x1>] = P<3x4>
+// K<3x3> * R<3x3> [I<3x3> , -C<3x1>] = P<3x4>
+// T<3x1> = -R<3x3> * C<3x1>
+inline void decompose(double const *P,
+                      double *K, double *R, double *T, double *C)
+{
+	CreateCvMatHead(_P,3,4,P);
+	CreateCvMatHead(_K,3,3,K);
+	CreateCvMatHead(_R,3,3,R);
+	double tmp[4];
+	CreateCvMatHead(_tmp,4,1,tmp);
+	//translation vector is camera Center in world coordinate system
+	//so it is C rather than T in opencv manual
+	cvDecomposeProjectionMatrix(&_P,&_K,&_R,&_tmp);
+	C[0]=tmp[0]/tmp[3];
+	C[1]=tmp[1]/tmp[3];
+	C[2]=tmp[2]/tmp[3];
+	CvMatHelper::mul(3,3,3,1,R,C,T); //T = -RC, C = -R'T
+	T[0]*=-1;
+	T[1]*=-1;
+	T[2]*=-1;
+}
+
+inline void compose(double const *K,
+                    double const *R, double const *T,
+                    double *P, bool TisC=false)
+{
+	double PP[12]= {0};
+	if(!TisC) { //K[R,T]
+		PP[0]=R[0], PP[1]=R[1], PP[2]= R[2], PP[3]= T[0];
+		PP[4]=R[3], PP[5]=R[4], PP[6]= R[5], PP[7]= T[1];
+		PP[8]=R[6], PP[9]=R[7], PP[10]=R[8], PP[11]=T[2];
+		CvMatHelper::mul(3,3,3,4,K,PP,P);
+	} else { //KR[I,-C], remember now T is C
+		P[0]=1, P[1]=0, P[2] =0, P[3] =-T[0];
+		P[4]=0, P[5]=1, P[6] =0, P[7] =-T[1];
+		P[8]=0, P[9]=0, P[10]=1, P[11]=-T[2];
+		CvMatHelper::mul(3,3,3,4,R,P,PP);
+		CvMatHelper::mul(3,3,3,4,K,PP,P);
+	}
+}
+
+inline void RotationMatrix_PH_CV(double *R)
+{
+	for(int i=1; i<3; ++i)
+		for(int j=0; j<3; ++j) {
+			R[i*3+j] *= -1;
+		}
+}
+
+inline void triangulate(const double x1, const double y1,
+                        const double x2, const double y2,
+                        const double P1[12], const double P2[12], double X[3])
+{
+	double A[12] = {
+		P1[0]-x1 *P1[8], P1[1]-x1 *P1[9], P1[2]-x1 *P1[10],
+		P1[4]-y1 *P1[8], P1[5]-y1 *P1[9], P1[6]-y1 *P1[10],
+		P2[0]-x2 *P2[8], P2[1]-x2 *P2[9], P2[2]-x2 *P2[10],
+		P2[4]-y2 *P2[8], P2[5]-y2 *P2[9], P2[6]-y2 *P2[10]
+	};
+	double b[4] = {
+		x1 *P1[11]-P1[3],
+		y1 *P1[11]-P1[7],
+		x2 *P2[11]-P2[3],
+		y2 *P2[11]-P2[7]
+	};
+
+	CreateCvMatHead(_A,4,3,A);
+	CreateCvMatHead(_b,4,1,b);
+	CreateCvMatHead(_X,3,1,X);
+	cvSolve(&_A,&_b,&_X, CV_SVD);
+}
+
+//compute plane pose (R and T) from K and Homography
+//H = s^(-1) * K * [r1,r2,T]
+//H maybe scaled by -1 in case det(H)<0
+inline void RTfromKH(const double K[9], double H[9],
+                     double R[9], double T[3])
+{
+	if(CvMatHelper::det(3,H)<0) {
+		CvMatHelper::scale(3,3,H,-1,H);
+	}
+	double invK[9];
+	double A[9];
+	CvMatHelper::inv(3,K,invK);
+	CvMatHelper::mul(3,3,3,3,invK,H,A);
+	double s = 1.0/sqrt(A[0]*A[0]+A[3]*A[3]+A[6]*A[6]);
+	CvMatHelper::scale(3,3,A,s,A);
+	double r1[3]= {A[0],A[3],A[6]};
+	double r2[3]= {A[1],A[4],A[7]};
+	double r3[3]= {0};
+	CvMatHelper::cross(r1,r2,r3);
+	R[0]=r1[0], R[1]=r2[0], R[2]=r3[0];
+	R[3]=r1[1], R[4]=r2[1], R[5]=r3[1];
+	R[6]=r1[2], R[7]=r2[2], R[8]=r3[2];
+	T[0]=A[2],  T[1]=A[5],  T[2]=A[8]; //translation
+}
+
+}//CameraHelper
+
+#endif

third_party/cv2cg/CvMatHelper.h

+#ifndef CVMATHELPER_H
+#define CVMATHELPER_H
+/*
+ *  Copyright (c) 2010  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* CvMatHelper.h
+   CvMat related helper functions */
+
+//standard include
+#include <iostream>
+#include <iomanip>
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <stdio.h>
+#include <time.h>
+//opencv include
+#include "OpenCVHelper.h"
+
+namespace CvMatHelper
+{
+#define CreateCvMatHead(h,r,c,p) CvMat h = cvMat(r,c,CV_64FC1, const_cast<double*>(p))
+#define IDX(m,n,N) ((m)*(N)+(n)) //N cols
+
+inline void identity(int n, double *A)
+{
+	memset(A, 0, sizeof(double)*n*n);
+	for(int i=0; i<n; ++i) {
+		A[i *n+i]=1.0;
+	}
+}
+
+inline void zeros(int m, int n, double *A)
+{
+	memset(A, 0, sizeof(double)*m*n);
+}
+
+// A<mxn> -> AT<nxm>
+inline void transpose(int m,  int n,
+                      double const *A, double *AT)
+{
+	CreateCvMatHead(_A,m,n,A);
+	CreateCvMatHead(_AT,n,m,AT);
+	cvTranspose(&_A,&_AT);
+}
+
+// R<amxbn> = A<amxan> * B<bmxbn>, an==bm
+inline void mul(int am, int an, int bm, int bn,
+                double const *A, double const *B, double *R)
+{
+	CreateCvMatHead(_A,am,an,A);
+	CreateCvMatHead(_B,bm,bn,B);
+	CreateCvMatHead(_R,am,bn,R);
+	cvMatMul(&_A,&_B,&_R);
+}
+
+// R<amxbn> = A<amxan>' * B<bmxbn>, am==bm
+inline void mulAtB(int am, int an, int bm, int bn,
+                   double const *A, double const *B, double *R)
+{
+	CreateCvMatHead(_A,am,an,A);
+	CreateCvMatHead(_B,bm,bn,B);
+	CreateCvMatHead(_R,an,bn,R);
+	cvGEMM(&_A,&_B,1,0,0,&_R,CV_GEMM_A_T);
+}
+
+// R<amxbn> = A<amxan> * B<bmxbn>', an==bn
+inline void mulABt(int am, int an, int bm, int bn,
+                   double const *A, double const *B, double *R)
+{
+	CreateCvMatHead(_A,am,an,A);
+	CreateCvMatHead(_B,bm,bn,B);
+	CreateCvMatHead(_R,am,bm,R);
+	cvGEMM(&_A,&_B,1,0,0,&_R,CV_GEMM_B_T);
+}
+
+// R<1x1> = x'<1xn> * A<nxn> * y<nx1>
+inline double mulxtAy(int n,
+                      double const *x, double const *A, double const *y)
+{
+	CreateCvMatHead(_A,n,n,A);
+	CreateCvMatHead(_y,n,1,y);
+	CreateCvMatHead(_xt,1,n,x);
+	double result;
+	CreateCvMatHead(_R,1,1,&result);
+	CvMat *tmp = cvCreateMat(n,1,CV_64FC1);
+	cvMatMul(&_A,&_y,tmp);
+	cvMatMul(&_xt,tmp,&_R);
+	cvReleaseMat(&tmp);
+	return result;
+}
+
+// R<mxn> = A<mxn> * s
+inline void scale(int m,  int n,
+                  double const *A, double s, double *R)
+{
+	int mn = m*n;
+	for(int i=0; i<mn; ++i) {
+		R[i]=A[i]*s;
+	}
+}
+
+// R<nxn> = A<nxn> ^ idx
+inline void pow(int n, double const *A, int idx, double *R)
+{
+	CreateCvMatHead(_A,n,n,A);
+	CreateCvMatHead(_R,n,n,R);
+	cvPow(&_A,&_R,idx);
+}
+
+// R<mxn> = A<mxn> + B<mxn>
+inline void add(int m, int n,
+                double const *A, double const *B, double *R)
+{
+	CreateCvMatHead(_A,m,n,A);
+	CreateCvMatHead(_B,m,n,B);
+	CreateCvMatHead(_R,m,n,R);
+	cvAdd(&_A,&_B,&_R);
+}
+
+// R<mxn> = A<mxn> - B<mxn>
+inline void sub(int m, int n,
+                double const *A, double const *B, double *R)
+{
+	CreateCvMatHead(_A,m,n,A);
+	CreateCvMatHead(_B,m,n,B);
+	CreateCvMatHead(_R,m,n,R);
+	cvSub(&_A,&_B,&_R);
+}
+
+inline double det(int n, double const *A)
+{
+	CreateCvMatHead(_A,n,n,A);
+	return cvDet(&_A);
+}
+
+inline double inv(int n, double const *A, double *Ainv, int method=CV_SVD)
+{
+	CreateCvMatHead(_A,n,n,A);
+	CreateCvMatHead(_Ainv,n,n,Ainv);
+	return cvInvert(&_A,&_Ainv,method);
+}
+
+// U<mxmn> * S<mnxmn> * VT<mnxn> = A<mxn>, mn=min(m,n)
+inline void svd(int m, int n,
+                double const *A, double *U, double *S, double *VT)
+{
+	int mn = std::min(m,n);
+	CreateCvMatHead(_A,m,n,A);
+	CreateCvMatHead(_U,m,mn,U);
+	CreateCvMatHead(_S,mn,mn,S);
+	CreateCvMatHead(_VT,mn,n,VT);
+	cvSVD(&_A,&_S,&_U,&_VT,CV_SVD_V_T);
+}
+
+// x<nx1> = arg(x) min ||A<mxn> * x - b<mx1>||
+// i.e. solve Ax=b
+inline int solve(int m, int n,
+                 double const *A, double const *b, double *x, int method=CV_SVD)
+{
+	CreateCvMatHead(_A,m,n,A);
+	CreateCvMatHead(_b,m,1,b);
+	CreateCvMatHead(_x,n,1,x);
+	return cvSolve(&_A,&_b,&_x,method);
+}
+
+// x<nx1> = arg(x) min ||A<mxn> * x||, s.j. ||x||=1
+inline void nullvector(int m, int n, double const *A, double *x)
+{
+	int mn = std::min(m,n);
+	CreateCvMatHead(_A,m,n,A);
+	CvMat *U = cvCreateMat(m,mn,CV_64FC1);
+	CvMat *S = cvCreateMat(mn,mn,CV_64FC1);
+	CvMat *V = cvCreateMat(n,mn,CV_64FC1);
+	cvSVD(&_A,S,U,V);
+	int end = mn-1;
+	for(int i=0; i<n; ++i) {
+		x[i] = CV_MAT_ELEM(*V, double, i, end);
+	}
+	cvReleaseMat(&U);
+	cvReleaseMat(&S);
+	cvReleaseMat(&V);
+}
+
+inline void cross(double const *A, double const *B, double *R)
+{
+	R[0]=A[1]*B[2]-A[2]*B[1];
+	R[1]=A[2]*B[0]-A[0]*B[2];
+	R[2]=A[0]*B[1]-A[1]*B[0];
+}
+
+inline double cross2D(double const *A, double const *B)
+{
+	return A[0]*B[1]-A[1]*B[0];
+}
+
+inline double dot(int am, int bm, double const *A, double const *B)
+{
+	double ret;
+	mulAtB(am,1,bm,1,A,B,&ret);
+	return ret;
+}
+
+//return the L2 norm of a vector (assume an==1 or am==1)
+inline double normL2(int am, int an, double const *A)
+{
+	int n = am*an;
+	return sqrt( dot(n,n,A,A) );
+}
+
+////////////////////////////////////////////
+///  Utils
+// convert a pointer to a continuous memory block
+// to a 2D array with "cols" columns in Precision type
+template<int cols, typename Precision=double *>
+struct FixMat {
+	typedef  Precision (*Type)[cols];
+
+	template<typename SrcType>
+	static Type ConvertType(SrcType src) {
+		return reinterpret_cast<Type>(src);
+	}
+};
+
+}//CvMatHelper
+
+#endif

third_party/cv2cg/DirHelper.h

+#ifndef DIRHELPER_H
+#define DIRHELPER_H
+/*
+ *  Copyright (c) 2010  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* DirHelper.h
+   modified from openscenegraph osgDB*/
+
+//standard include
+#include <iostream>
+#include <iomanip>
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <stdio.h>
+#include <time.h>
+
+namespace DirHelper
+{
+
+//dir
+inline std::string getFileDir(const std::string &fileName)
+{
+	std::string::size_type slash1 = fileName.find_last_of('/');
+	std::string::size_type slash2 = fileName.find_last_of('\\');
+	if (slash1==std::string::npos) {
+		if (slash2==std::string::npos) {
+			return std::string();
+		}
+		return std::string(fileName,0,slash2+1);
+	}
+	if (slash2==std::string::npos) {
+		return std::string(fileName,0,slash1+1);
+	}
+	return std::string(fileName, 0, 1 + slash1>slash2 ?  slash1 : slash2);
+}
+
+// no ext, pure name
+inline std::string getNameNoExtension(const std::string &str)
+{
+	std::string::size_type begin = str.find_last_of('\\');
+	std::string::size_type last = str.find_last_of('.');
+	if(last == str.npos) {
+		last = str.length()-1;
+	}
+	if( (begin == str.npos) ||
+	        (begin > last) ) {
+		return str;
+	} else {
+		return str.substr(begin+1, last-begin-1);
+	}
+}
+
+inline std::string getNameWithExtension(const std::string &fileName)
+{
+	std::string::size_type slash1 = fileName.find_last_of('/');
+	std::string::size_type slash2 = fileName.find_last_of('\\');
+	if (slash1==std::string::npos) {
+		if (slash2==std::string::npos) {
+			return fileName;
+		}
+		return std::string(fileName.begin()+slash2+1,fileName.end());
+	}
+	if (slash2==std::string::npos) {
+		return std::string(fileName.begin()+slash1+1,fileName.end());
+	}
+	return std::string(fileName.begin()+(slash1>slash2?slash1:slash2)+1,fileName.end());
+}
+
+inline std::string getFileExtensionNoDot(const std::string &fileName)
+{
+	std::string::size_type dot = fileName.find_last_of('.');
+	if (dot==std::string::npos) {
+		return std::string("");
+	}
+	return std::string(fileName.begin()+dot+1,fileName.end());
+}
+
+inline std::string getFileExtensionWithDot(const std::string &fileName)
+{
+	std::string::size_type dot = fileName.find_last_of('.');
+	if (dot==std::string::npos) {
+		return std::string("");
+	}
+	return std::string(fileName.begin()+dot,fileName.end());
+}
+
+}//DirHelper
+
+#endif

third_party/cv2cg/ESMHelper.h

+#pragma once
+/*
+ *  Copyright (c) 2011  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* ESMHelper.h
+   wrapper for ESM homography tracking refinement algorithm */
+
+#include <iostream>
+#include <vector>
+#include <string>
+#include <stdio.h>
+#include <stdlib.h>
+#include "Log.h"
+#include "OpenCVHelper.h"
+
+extern "C" {
+#include "ESMlibry.h"
+}
+
+using namespace cv;
+using namespace std;
+
+//!!! data still stored in m, and m will be changed to CV_32F if it is not
+inline void Mat2imageStruct(Mat& m, imageStruct& I)
+{
+	if(m.type()!=CV_32FC1) {
+		if(m.channels()>1) {
+			Mat gm;
+			cvtColor(m,gm,CV_RGB2GRAY);
+			gm.convertTo(m,CV_32F);
+		} else {
+			m.convertTo(m,CV_32F);
+		}
+	}
+
+	I.data = (float *)(m.data);
+	I.rows = m.rows;
+	I.cols = m.cols;
+}
+
+//!!! data will be copied to m
+inline void imageStruct2Mat(imageStruct& I, Mat& m)
+{
+	Mat mI(I.rows,I.cols,CV_32FC1,I.data);
+	mI.copyTo(m);
+	m.convertTo(m,CV_8U);
+}
+
+struct ESMKernel {
+	trackStruct T;
+	Mat RefImg;   //internally hold the reference/template image
+	bool inited;
+	int px,py,sx,sy;
+
+	ESMKernel() {inited=false;}
+	~ESMKernel() {
+		if(inited) {
+			FreeTrack(&T);
+		}
+	}
+
+	inline bool init(const Mat& refimg,
+	                 int posx, int posy,
+	                 int sizex, int sizey,
+	                 int maxIter=5, int maxPrec=2) {
+		px=posx; py=posy;
+		sx=sizex; sy=sizey;
+
+		if(inited) {
+			FreeTrack(&T);
+		}
+		refimg.copyTo(RefImg);
+		imageStruct I;
+		Mat2imageStruct(RefImg, I);
+		// Memory allocation for the tracking
+		if (MallTrack (&T, &I, posx, posy, sizex, sizey, maxIter, maxPrec)) {
+			return inited = false;
+		}
+		return inited = true;
+	}
+
+	inline bool run(imageStruct& I) {
+		if(!inited) {
+			cout<<"[ESMKernel::run] please init the ESMKernel first!"<<endl;
+			return false;
+		}
+
+		// Perform the tracking
+		if (MakeTrack (&T, &I)) {
+			return false;
+		}
+		return true;
+	}
+
+	inline void setH(double const H[9]) {
+		for(int i=0; i<9; i++)
+			T.homog[i] = H[i];
+	}
+
+	inline void getH(double H[9]) const {
+		for(int i=0; i<9; i++)
+			H[i] = T.homog[i];
+	}
+
+	template<typename Iterator>
+	void setH(Iterator itr) {
+		for(int i=0; i<9; ++i, ++itr)
+			T.homog[i] = (*itr);
+	}
+
+	template<typename Iterator>
+	void getH(Iterator itr) const {
+		for(int i=0; i<9; ++i, ++itr)
+			(*itr) = T.homog[i];
+	}
+};
+
+struct ESMTracker {
+	ESMKernel kernel;
+	Mat CurImg; //internally hold the image being tracked
+
+	// miter: the number of iterations of the ESM algorithm (>= 1);
+	// mprec: the precision of the ESM algorithm (1..10)
+	// low precision = 1, high precision = 10;
+	// miter and mprec should be chosen depending on the available
+	// computation time;
+	// For low-speed PCs or high video framerate choose low miter and low mprec.
+	// (posx,posy) The window position (upper left corner)
+	// The image coordinates start from (0,0)
+	// The window size sizex,sizey
+	inline bool init(const Mat& refimg,
+	                 int posx, int posy,
+	                 int sizex, int sizey,
+	                 int maxIter=5, int maxPrec=2) {
+		if (!kernel.init(refimg,posx,posy,sizex,sizey,maxIter,maxPrec)) {
+			cout<<"[ESMTracker] failed to inited."<<endl;
+			return false;
+		}
+		cout<<"[ESMTracker] inited."<<endl;
+		return true;
+	}
+
+	inline bool run(Mat& curimg, bool keepCurrent=true) {
+		imageStruct I;
+		if(keepCurrent) {
+			curimg.copyTo(CurImg);
+			Mat2imageStruct(CurImg, I);
+		} else {
+			Mat2imageStruct(curimg, I);
+		}
+		return kernel.run(I);
+	}
+
+	inline void setH(double const H[9]) {
+		kernel.setH(H);
+	}
+
+	inline void getH(double H[9]) const {
+		kernel.getH(H);
+	}
+
+	template<typename Iterator>
+	void setH(Iterator itr) {
+		kernel.setH(itr);
+	}
+
+	template<typename Iterator>
+	void getH(Iterator itr) const {
+		kernel.getH(itr);
+	}
+};
+
+struct ESMMultipleTracker {
+	vector<ESMKernel> kArr;
+	Mat CurImg; //internally hold the image being tracked
+
+	inline bool init(const Mat& refimg,
+	                 int posx, int posy,
+	                 int sizex, int sizey,
+	                 int maxIter=5, int maxPrec=2) {
+		kArr.push_back(ESMKernel());
+		ESMKernel& kernel = kArr[kArr.size()-1];
+		if (!kernel.init(refimg,posx,posy,sizex,sizey,maxIter,maxPrec)) {
+			cout<<"[ESMMultipleTracker] new("<<kArr.size()-1<<") kernel failed to inited."<<endl;
+			kArr.resize(kArr.size()-1);
+			return false;
+		}
+		cout<<"[ESMMultipleTracker] new("<<kArr.size()-1<<") kernel inited."<<endl;
+		return true;
+	}
+
+	inline bool run(Mat& curimg, bool keepCurrent=true) {
+		imageStruct I;
+		if(keepCurrent) {
+			curimg.copyTo(CurImg);
+			Mat2imageStruct(CurImg, I);
+		} else {
+			Mat2imageStruct(curimg, I);
+		}
+
+		int cnt=(int)kArr.size();
+		int oi=0;
+		for(int i=0; i<cnt;++oi) {
+			ESMKernel& kernel = kArr[i];
+			if(!kernel.run(I)) {
+				swap(kArr[i],kArr[cnt-1]);
+				--cnt;
+				cout<<"[ESMMultipleTracker] kernel("
+					<<oi<<") lost track, deleted."<<endl;
+			} else {
+				++i;
+			}
+		}
+		bool ret = cnt==(int)kArr.size();
+		if(cnt==0) kArr.clear();
+		else kArr.resize(cnt);
+		return ret;
+	}
+};

third_party/cv2cg/FilterHelper.h

+#pragma once
+/*
+ *  Copyright (c) 2011  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* FilterHelper.h
+   some filter functions */
+
+//standard include
+#include <iostream>
+#include <iomanip>
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <stdio.h>
+#include <time.h>
+
+namespace FilterHelper
+{
+
+template<typename T>
+inline void stablize(int dim,
+                     T const *newObservation, T *filtered,
+                     T bufferLen=1.0f, T amplification=500.0f)
+{
+	for(int i=0; i<dim; ++i) {
+		filtered[i] += amplification;
+		double tmp = newObservation[i] + amplification;
+		filtered[i] = (T) (sqrt((filtered[i] * filtered[i] * bufferLen
+		                         + tmp * tmp) / (1 + bufferLen)));
+		filtered[i] -= amplification;
+	}
+}
+
+}//end of namespace FilterHelper

third_party/cv2cg/IOHelper.h

+#pragma once
+/*
+ *  Copyright (c) 2010  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* IOHelper.h */
+
+//standard include
+#include <iostream>
+#include <iomanip>
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <stdio.h>
+#include <time.h>
+
+#include "Log.h" // please include "Log.hxx" in and only in your main.cpp
+
+namespace IOHelper
+{
+#define IDX(m,n,N) ((m)*(N)+(n)) //N cols
+
+//read one valid line, i.e. non-empty line
+//return false if no valid line, i.e. end-of-file
+inline bool readValidLine(std::istream &in,
+                          std::string &line)
+{
+	line = "";//clear;
+	bool haschar = false;
+	while(in) {
+		std::string str;
+		std::getline(in, str);
+		if(str.length()==0) {
+			continue;
+		}
+		haschar = true;
+
+		//take care of CR/LF, see details:
+		// http://www.codeguru.com/forum/showthread.php?t=253826
+		char lastchar = str[str.length()-1];
+		if(lastchar=='\n' || lastchar=='\r') {
+			str.resize(str.length()-1);
+		}
+		if(str.length()==0) {
+			continue;
+		}
+
+		line = str;
+		break;
+	}
+	return line.length()!=0 || haschar;
+}
+
+//read calibration matrix from stream
+//format: (only numbers and white characters, no other stuff)
+//K[0] K[1] K[2]
+//K[3] K[4] K[5]
+//K[6] K[7] K[8]
+template<typename Precision>
+bool readCalibFile(std::istream &in, Precision K[9]) {
+	for(int i=0; i<9; ++i) {
+		double val;
+		in >> val;
+		K[i] = (Precision) val;
+	}
+	return true;
+}
+
+/* print the given n x m matrix */
+template<typename Precision>
+void print(int m, int n, Precision *A, const char *mat_name=0)
+{
+	int i, j;
+
+	if(mat_name) {
+		printf("%s = \n\n", mat_name);
+	}
+
+	for (i = 0; i < m; i++) {
+		printf("  ");
+		for (j = 0; j < n; j++) {
+			printf(" % 10f", (float)A[i * n + j]);
+		}
+		printf("\n");
+	}
+	printf("\n");
+}
+
+/* Read a matrix from a file */
+template<typename Precision>
+bool ReadFile(int m, int n, Precision const *matrix, const char *fname)
+{
+	FILE *f = NULL;
+	f = fopen(fname, "r");
+	int i;
+
+	if (f == NULL) {
+		TagE("In reading matrix %s\n", fname);
+		return false;
+	}
+
+	for (i = 0; i < m * n; i++) {
+		fscanf_s(f, "%lf", matrix + i);
+	}
+
+	fclose(f);
+	return true;
+}
+
+/* Write a matrix to a file */
+template<typename Precision>
+bool WriteFile(int m, int n, Precision const *matrix, const char *fname)
+{
+	FILE *f = NULL;
+	f = fopen(fname, "w");
+	int i, j, idx;
+
+	if (f == NULL) {
+		TagE("In writing matrix to %s\n", fname);
+		return false;
+	}
+
+	idx = 0;
+	for (i = 0; i < m; i++) {
+		for (j = 0; j < n; j++, idx++) {
+			fprintf(f, "%0.16e ", matrix[idx]);
+		}
+		fprintf(f, "\n");
+	}
+
+	fclose(f);
+	return true;
+}
+
+//Utils for using c++ stream to print matrix
+//e.g.
+//	cout<<"R=\n"<<PrintMat<>(3,3,R)<<endl;
+template<unsigned int iosflag=std::ios::scientific,
+         typename Precision=double>
+struct PrintMat {
+	int cols,rows;
+	Precision const *p;
+	PrintMat(int r, int c, Precision const *ptr) {
+		cols=c;
+		rows=r;
+		p=ptr;
+	}
+	friend inline std::ostream &operator<<(
+	    std::ostream &o, const PrintMat &m) {
+		o.setf((std::ios_base::fmtflags)iosflag);
+		for(int i=0; i<m.rows; ++i) {
+			for(int j=0; j<m.cols; ++j) {
+				o << m.p[IDX(i,j,m.cols)] << " ";
+			}
+			o << std::endl;
+		}
+		o.unsetf((std::ios_base::fmtflags)iosflag);
+		return o;
+	}
+};
+
+}//end of IOHelper

third_party/cv2cg/ImageHelper.h

+#ifndef IMAGEHELPER_H
+#define IMAGEHELPER_H
+/*
+ *  Copyright (c) 2010  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* ImageHelper.h
+   Image Processing related helper functions*/
+
+//standard include
+#include <iostream>
+#include <iomanip>
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <stdio.h>
+#include <time.h>
+//opencv include
+#include "OpenCVHelper.h"
+
+namespace ImageHelper
+{
+#define CV_RED		Scalar(255,0,0)
+#define CV_GREEN	Scalar(0,255,0)
+#define CV_BLUE		Scalar(0,0,255)
+#define CV_WHITE	Scalar(255,255,255)
+#define CV_BLACK	Scalar(0,0,0)
+#define CV_GRAY		Scalar(128,128,128)
+
+// generate pseudocolor look up table
+// maxcolors: required max number of colors
+inline std::vector<CvScalar> pseudocolor(int maxcolors)
+{
+	//find proper number of bit_per_channle
+	//maxcolors = std::min(1<<30,maxcolors);
+	int bit_per_channle, bits_total, color_num;
+	for(bit_per_channle=1; bit_per_channle<11; ++bit_per_channle) {
+		bits_total = 3*bit_per_channle;
+		color_num = 1 << bits_total;
+		if(color_num>=maxcolors || bit_per_channle==10) {
+			--bit_per_channle;
+			bits_total = 3*bit_per_channle;
+			color_num = 1 << bits_total;
+			break;
+		}
+	}
+
+	std::vector<CvScalar> lut(color_num);
+
+	for(int c = 0; c < color_num; c++) {
+		int r = 0;
+		int g = 0;
+		int b = 0;
+		for(int k = 0; k < bits_total; ) {
+			b = (b << 1) + ((c >> k++) & 1);
+			g = (g << 1) + ((c >> k++) & 1);
+			r = (r << 1) + ((c >> k++) & 1);
+		}
+		r = r << (8 - bit_per_channle);
+		g = g << (8 - bit_per_channle);
+		b = b << (8 - bit_per_channle);
+
+		lut[c].val[0]=r;
+		lut[c].val[1]=g;
+		lut[c].val[2]=b;
+	}
+
+	return lut;
+}
+
+}//ImageHelper
+
+#endif

third_party/cv2cg/Log.h

+#ifndef LOG_H
+#define LOG_H
+/*
+ *  Copyright (c) 2010  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* Log.h */
+
+#include <stdio.h>
+
+namespace Log
+{
+//switch them
+extern bool debug, info, error, warn;
+extern FILE *logfile;
+
+inline static bool tag(const char *tag, const char *level, bool control)
+{
+	if(control && logfile) {
+		fprintf(logfile, "[%s %s] ", tag, level);
+	}
+	return true;
+}
+}//Log
+
+#define LogD(...) if(Log::debug) fprintf(Log::logfile,__VA_ARGS__)
+#define LogI(...) if(Log::info) fprintf(Log::logfile,__VA_ARGS__)
+#define LogE(...) if(Log::error) fprintf(Log::logfile,__VA_ARGS__)
+#define LogW(...) if(Log::warn) fprintf(Log::logfile,__VA_ARGS__)
+#define TagD(...) Log::tag(__FUNCTION__, "debug", Log::debug),\
+	Log::debug?fprintf(Log::logfile,__VA_ARGS__):0
+#define TagI(...) Log::tag(__FUNCTION__, "info", Log::info),\
+    Log::info?fprintf(Log::logfile,__VA_ARGS__):0
+#define TagE(...) Log::tag(__FUNCTION__, "error", Log::error),\
+	Log::error?fprintf(Log::logfile,__VA_ARGS__):0
+#define TagW(...) Log::tag(__FUNCTION__, "warn", Log::warn),\
+	Log::warn?fprintf(Log::logfile,__VA_ARGS__):0
+
+#endif

third_party/cv2cg/Log.hxx

+#pragma once
+/* 
+ *  Copyright (c) 2010  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* Log.inc
+   please only include this file in only one of your .cpp files,
+   usually include it in main.cpp */
+
+#include <stdio.h>
+
+namespace Log {
+	//switch them
+	bool debug=false, info=true, error=true, warn=true;
+	FILE *logfile = stdout;
+}

third_party/cv2cg/MatrixManip_deprecated.h

+#pragma once
+
+#include <iostream>
+#include <iomanip>
+using namespace std;
+
+typedef double (*VecMat)[1];
+typedef const double (*ConstVecMat)[1];
+
+inline VecMat Vector2Matrix(double M[])
+{
+	VecMat ret = reinterpret_cast<double ( *)[1]>(M);
+	return ret;
+}
+
+inline ConstVecMat Vector2Matrix(const double M[])
+{
+	ConstVecMat ret = reinterpret_cast<const double ( *)[1]>(M);
+	return ret;
+}
+
+template<unsigned int a, unsigned int b>
+std::ostream &Matrix_Display(std::ostream &o, const double M[a][b])
+{
+	o << setiosflags(ios::scientific);
+
+	for(unsigned int i=0; i<a; ++i) {
+		for(unsigned int j=0; j<b; ++j) {
+			o << M[i][j] << " " ;
+		}
+		o << std::endl;
+	}
+	return o;
+}
+
+template<unsigned int a, unsigned int b, unsigned int c>
+void Matrix_Times(const double A[a][b], const double B[b][c], double C[a][c])
+{
+	for(unsigned int i=0; i<a; ++i)
+		for(unsigned int j=0; j<c; ++j) {
+			C[i][j]=0;
+			for(unsigned int k=0; k<b; ++k) {
+				C[i][j] += A[i][k] * B[k][j];
+			}
+		}
+}
+
+template<unsigned int a, unsigned int b>
+void Matrix_Transpose(const double A[a][b], double At[b][a])
+{
+	for(unsigned int i=0; i<a; ++i)
+		for(unsigned int j=0; j<b; ++j) {
+			At[j][i] = A[i][j];
+		}
+}

third_party/cv2cg/OSGHelper.h

+#pragma once
+/*
+ *  Copyright (c) 2011  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* OSGHelper.h
+   a set of helper functions for easier access to openscenegraph */
+
+#include <osgDB/ReadFile>
+#include <osgDB/WriteFile>
+
+#include <osgViewer/Viewer>
+#include <osgGA/TrackballManipulator>
+#include <osgViewer/ViewerEventHandlers>
+
+#include <osg/Material>
+#include <osg/Geode>
+#include <osg/Geometry>
+//#include <osg/Array>
+#include <osg/BlendFunc>
+#include <osg/Depth>
+#include <osg/MatrixTransform>
+#include <osg/Camera>
+
+#include <osgText/Text>
+#include <osg/Texture2D>
+#include <osg/Image>
+
+namespace OSGHelper
+{
+
+inline osg::Geode* createGeodeFromImage(osg::Image* image, bool flip=false)
+{
+	if (image) {
+		float s = image->s();
+		float t = image->t();
+		if (s>0 && t>0) {
+			float y = t;
+			float x = s;
+
+			float texcoord_y_b = flip ? 0.0f : 1.0f;
+			float texcoord_y_t = flip ? 1.0f : 0.0f;
+
+			// set up the texture.
+			osg::Texture2D* texture = new osg::Texture2D;
+			texture->setResizeNonPowerOfTwoHint(false);
+			texture->setImage(image);
+
+			// set up the geoset.
+			osg::Geometry* geom = new osg::Geometry;
+			osg::StateSet* dstate = geom->getOrCreateStateSet();
+			dstate->setMode(GL_CULL_FACE,osg::StateAttribute::OFF);
+			dstate->setMode(GL_LIGHTING,osg::StateAttribute::OFF);
+			dstate->setTextureAttributeAndModes(0, 
+				texture,osg::StateAttribute::ON);
+
+			osg::Vec3Array* coords = new osg::Vec3Array(4);
+			(*coords)[0].set(0,y,0);
+			(*coords)[1].set(0,0,0);
+			(*coords)[2].set(x,0,0);
+			(*coords)[3].set(x,y,0);
+			geom->setVertexArray(coords);
+
+			osg::Vec2Array* tcoords = new osg::Vec2Array(4);
+			(*tcoords)[0].set(0.0f,texcoord_y_t);
+			(*tcoords)[1].set(0.0f,texcoord_y_b);
+			(*tcoords)[2].set(1.0f,texcoord_y_b);
+			(*tcoords)[3].set(1.0f,texcoord_y_t);
+			geom->setTexCoordArray(0,tcoords);
+
+			osg::Vec4Array* colours = new osg::Vec4Array(1);
+			(*colours)[0].set(1.0f,1.0f,1.0,1.0f);
+			geom->setColorArray(colours);
+			geom->setColorBinding(osg::Geometry::BIND_OVERALL);
+
+			geom->addPrimitiveSet(new osg::DrawArrays(
+				osg::PrimitiveSet::QUADS,0,4));
+
+			// set up the geode.
+			osg::Geode* geode = new osg::Geode;
+			geode->addDrawable(geom);
+
+			return geode;
+		}
+		return NULL;
+	}
+	return NULL;
+}
+
+
+struct ARVideoBackground : public osg::Camera {
+	osg::ref_ptr<osg::Image> frame;
+	int width,height; //image width and height
+
+	ARVideoBackground(osg::Image* img, bool flip=false) {
+		frame = img;
+		width = img?img->s():1;
+		height = img?img->t():1;
+
+		//SET UP HUD
+		// set the projection matrix
+		setProjectionMatrix(osg::Matrix::ortho2D(0,width,0,height));
+		// set the view matrix
+		setReferenceFrame(osg::Transform::ABSOLUTE_RF);
+		setViewMatrix(osg::Matrix::identity());
+		// only clear the depth buffer
+		setClearMask(GL_DEPTH_BUFFER_BIT);
+		// draw subgraph after main camera view.
+		setRenderOrder(osg::Camera::NESTED_RENDER);
+		getOrCreateStateSet()->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF);
+		getOrCreateStateSet()->setRenderBinDetails(-1,"RenderBin");
+		setAllowEventFocus(false);
+
+		//add geometry
+		osg::Geode* geode = createGeodeFromImage(img,flip);
+		if(geode) {
+			addChild(geode);
+		} else {
+			std::cout<<"[ARVideoNode] No Valid Image!"<<std::endl;
+		}
+	}
+};
+
+struct ARSceneRoot : public osg::Camera {
+	ARSceneRoot() {
+		setReferenceFrame(osg::Transform::ABSOLUTE_RF);
+		// only clear the depth buffer
+		setClearMask(GL_DEPTH_BUFFER_BIT);
+		getOrCreateStateSet()->setRenderBinDetails(1,"RenderBin");
+		setAllowEventFocus(false);
+	}
+};
+
+}
+
+namespace helper {
+using namespace OSGHelper;
+}

third_party/cv2cg/OpenCV2OSG.h

+#pragma once
+/*
+ *  Copyright (c) 2011  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* OpenCV2OSG.h
+   a set of helper functions for conversion between opencv and openscenegraph */
+
+#include "OpenCVHelper.h"
+#include "OSGHelper.h"
+
+namespace helper {
+
+using namespace cv;
+using namespace std;
+
+inline bool cvmat2osgimage(Mat& cvImg, osg::Image* osgImg, bool bflip=false)
+{
+	if(!osgImg) {
+		cout<<"[cvmat2osgimage] no valid osg::Image!"<<endl;
+		return false;
+	}
+	// Flip image from top-left to bottom-left origin
+	if(bflip) flip(cvImg,cvImg,0);
+	osgImg->setWriteHint(osg::Image::NO_PREFERENCE);
+
+	if(cvImg.channels() == 3)
+	{
+		// Convert from BGR to RGB color format
+		// cvtColor( cvImg, cvImg, CV_BGR2RGB );
+
+		osgImg->setImage(
+			cvImg.cols, //s
+			cvImg.rows, //t
+			3, //r
+			GL_LINE_STRIP, //GLint internalTextureformat, (, 0x0003)
+			GL_BGR, // GLenum pixelFormat, (GL_RGB, 0x1907)
+			GL_UNSIGNED_BYTE, // GLenum type, (GL_UNSIGNED_BYTE, 0x1401)
+			cvImg.data, // unsigned char* data
+			osg::Image::NO_DELETE // AllocationMode mode (shallow copy)
+			);//int packing=1); (???)
+
+		return true;
+	} else if(cvImg.channels() == 1) {
+		osgImg->setImage(
+			cvImg.cols, //s
+			cvImg.rows, //t
+			1, //r
+			GL_LINE_STRIP, //GLint internalTextureformat, (GL_LINE_STRIP, 0x0003)
+			GL_LUMINANCE, // GLenum pixelFormat
+			GL_UNSIGNED_BYTE, // GLenum type
+			cvImg.data, // unsigned char* data
+			osg::Image::NO_DELETE // AllocationMode mode (shallow copy)
+			);//int packing=1); (???)
+		return true;
+	}
+
+	cout<<"[cvmat2osgimage] unrecognized image type!"<<endl;
+	return false;
+}
+
+}

third_party/cv2cg/OpenCVHelper.h

+#ifndef OPENCVHELPER_H
+#define OPENCVHELPER_H
+/*
+ *  Copyright (c) 2011  Chen Feng (cforrest (at) umich.edu)
+ *    and the University of Michigan
+ *
+ *  This program 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 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program 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.
+ *
+ */
+
+/* OpenCVHelper.h