/**
@file?main.cpp
@date?2017/09/03
@author?tkwoo（wtk1101@gmail.com）.
@brief?main?file
*/

#include?
#include?
#include?
#include?
?
#include?“StereoVisionForADAS.h“
#include?“SurfaceNormal.h“

//?#define?VIZ
#ifdef?VIZ
#include?
#endif

using?namespace?std;
using?namespace?cv;

int?main（）
{
#ifdef?VIZ
	viz::Viz3d?plot3d（“Coordinate?frame“）;
	plot3d.showWidget（“Coordinate?Widget“?viz::WCoordinateSystem（））;

	viz::Viz3d?vizPlotVec（“vector?direction“）;
	vizPlotVec.showWidget（“Coordinate?Widget“?viz::WCoordinateSystem（））;
#endif

	StereoCamParam_t?objParam?=?CStereoVisionForADAS::InitStereoParam（KITTI）;
	CStereoVisionForADAS?objStereoVision（objParam）;?//stixel?constructor
	CSuNoVeMap?objSuNoVeMap（objParam）;?//SNV

	int?cntframe?=?0;
	char?chLeftImageName[150]?=?{};
	char?chRightImageName[150]?=?{};
	char?chOxtsName[150]?=?{};
	int?nWaitTime?=?0;
	
	while?（1）{//?（!plot3d.wasStopped（））{
		cout?<ame?<
		sprintf（chLeftImageName?“./data/left/%010d.png“?cntframe）;
		sprintf（chRightImageName?“./data/right/%010d.png“?cntframe）;

		cntframe++;

		/*fstream?fs;
		fs.open（chOxtsName）;
		double?dTemp;
		fs?>>?dTemp?>>?dTemp?>>?dTemp?>>?dTemp?>>?dTemp;
		printf（“%.1lfdeg?“?-dTemp*180/PI）;*/
		//cout?<
		Mat?imgLeft?=?imread（chLeftImageName?1）;
		Mat?imgRight?=?imread（chRightImageName?1）;
		if?（imgLeft.empty（））?{waitKey（）;break;}
		
		//imshow（“right“?imgRight）;
		
		int64?t?=?getTickCount（）;

		//?procesing
		objStereoVision.objectness（imgLeft?imgRight）;

		Mat?imgDisp8;
		objStereoVision.m_matDisp16.convertTo（imgDisp8?CV_8U?255?/?（objParam.m_nNumberOfDisp*16.））;

		Mat?imgG;
		bitwise_and（objStereoVision.m_imgGround?imgDisp8?imgG）;
		//?surface?normal?prcessing
		//objSuNoVeMap.Compute（objStereoVision.m_imgDisp8）;
		//objSuNoVeMap.Compute（imgG）;
		objSuNoVeMap.Compute（imgDisp8）;
		printf（“Time?elapsed:?%.3fms\n“?（getTickCount（）?-?t）?*?1000?/?getTickFrequency（））;
		
		Mat?imgResult?=?imgLeft.clone（）;
		Mat?imgStixel?=?imgLeft.clone（）;

		Mat?imgDispColor;
		applyColorMap（imgDisp8?imgDispColor?COLORMAP_OCEAN）;
		//cout?<		for?（int?i?=?0;?i?			Point?ptEnd;
			Point3d?ptDir?=?0.15*objSuNoVeMap.m_objValidSNVs[i].m_vec3dDirection?/?objSuNoVeMap.m_objValidSNVs[i].dScale;
			Point3d?ptEnd3D?=?ptDir?+?objSuNoVeMap.m_objValidSNVs[i].m_ptPositionRealWorld;
			ptEnd.x?=?ptEnd3D.x?*?objParam.objCamParam.m_dFocalLength?/?ptEnd3D.z?+?（objParam.objCamParam.m_sizeSrc.width?/?2）;
			ptEnd.y?=?ptEnd3D.y?*?objParam.objCamParam.m_dFocalLength?/?ptEnd3D.z?+?（objParam.objCamParam.m_sizeSrc.height?/?2）;
			line（imgDispColor?objSuNoVeMap.m_objValidSNVs[i].m_ptPositionImage?ptEnd?Scalar（0?255?0））;
		}
		for?（int?i?=?0;?i?