Ejemplos de uv_right en C++ (Cpp)

Ejemplo n.º 1

Archivo: face_detector.cpp Proyecto: Atom-machinerule/OpenQbo

float FaceDetector::calcDistanceToHead(cv::Mat& head, cv::KalmanFilter& kalman_filter)
{
	float u_left=kalman_filter.statePost.at<float>(0,0)-head.cols/2;
	cv::Mat uv_left(3,1,CV_32F);
	uv_left.at<float>(0,0)=u_left;
	uv_left.at<float>(1,0)=kalman_filter.statePost.at<float>(1,0);
	uv_left.at<float>(2,0)=1;
	float u_right=u_left+head.cols;
	cv::Mat uv_right(3,1,CV_32F);
	uv_right.at<float>(0,0)=u_right;
	uv_right.at<float>(1,0)=kalman_filter.statePost.at<float>(1,0);
	uv_right.at<float>(2,0)=1;


	cv::Mat cart_left=p_.inv()*uv_left;
	cv::Mat cart_right=p_.inv()*uv_right;


	cart_left=cart_left/sqrt(cart_left.dot(cart_left));
	cart_right=cart_right/sqrt(cart_right.dot(cart_right));

	float theta=acos(cart_left.dot(cart_right));
	float l=(HEAD_SIZE/2)/tan(theta/2);
	return l;
}

Ejemplo n.º 2

Archivo: new_face_detector.cpp Proyecto: hobie88/tutino.indigo

float FaceDetector::calcDistanceToHead(cv::Mat& head, cv::KalmanFilter& kalman_filter)
{
	float u_left=kalman_filter.statePost.at<float>(0,0)-head.cols/2;
	cv::Mat uv_left(3,1,CV_32F);
	uv_left.at<float>(0,0)=u_left;
	uv_left.at<float>(1,0)=kalman_filter.statePost.at<float>(1,0);
	uv_left.at<float>(2,0)=1;
	float u_right=u_left+head.cols;
	cv::Mat uv_right(3,1,CV_32F);
	uv_right.at<float>(0,0)=u_right;
	uv_right.at<float>(1,0)=kalman_filter.statePost.at<float>(1,0);
	uv_right.at<float>(2,0)=1;


	cv::Mat cart_left=p_.inv()*uv_left;
	cv::Mat cart_right=p_.inv()*uv_right;
//	std::cout << "cart left: " << cart_left << std::endl;

	cart_left=cart_left/sqrt(cart_left.dot(cart_left));
	cart_right=cart_right/sqrt(cart_right.dot(cart_right));

//	std::cout << "cart left after: " << cart_left << std::endl;
//	std::cout << "acos argument: " << cart_left.dot(cart_right) << std::endl;
	float theta=acos(cart_left.dot(cart_right));
	float l=(HEAD_SIZE/2)/tan(theta/2);
	//ROS_ERROR("THETA= %f  --- DISTANCE TO HEAD= %f",theta,l);
	return l;
}

Ejemplo n.º 3

Archivo: face_detector.cpp Proyecto: saiwaiyanyu/runbot_number0

float FaceDetector::calcDistanceToHead(cv::Mat& head, cv::KalmanFilter& kalman_filter)
{
	float u_left=kalman_filter.statePost.at<float>(0,0)-head.cols/2;
	cv::Mat uv_left(3,1,CV_32F);
	uv_left.at<float>(0,0)=u_left;
	uv_left.at<float>(1,0)=kalman_filter.statePost.at<float>(1,0);
	uv_left.at<float>(2,0)=1;
	float u_right=u_left+head.cols;
	cv::Mat uv_right(3,1,CV_32F);
	uv_right.at<float>(0,0)=u_right;
	uv_right.at<float>(1,0)=kalman_filter.statePost.at<float>(1,0);
	uv_right.at<float>(2,0)=1;


	cv::Mat cart_left=p_.inv()*uv_left;
	cv::Mat cart_right=p_.inv()*uv_right;
//	ROS_ERROR("!!!!!!!!!!!!!!!!!!!!!printf all the matrixs!!!!!!!!!!!\n");
//        std::cout<< "*********printf all the matrixs************" <<endl;
//	std::cout<< "uv_left"<<endl << uv_left <<endl<<endl;
//        std::cout<< "uv_ritht"<<endl << uv_right <<endl<<endl;
//        std::cout<< "p_.inv()"<<endl << p_.inv() <<endl<<endl;
//        std::cout<< "cart_left"<<endl << cart_left <<endl<<endl;
//        std::cout<< "cart_right"<<endl << cart_right <<endl<<endl;

//        ROS_ERROR("p_ 3*4");  PrintfMatrix(p_);
//        ROS_ERROR("uv_left 3*1");   PrintfMatrix(uv_left);
//        ROS_ERROR("uv_right 3*1");  PrintfMatrix(uv_right);
//        ROS_ERROR("cart_left 4*1"); PrintfMatrix(cart_left);
//        ROS_ERROR("cart_right 4*1");PrintfMatrix(cart_right);


	cart_left=cart_left/sqrt(cart_left.dot(cart_left));
	cart_right=cart_right/sqrt(cart_right.dot(cart_right));

	//ROS_ERROR("costheta:%f", cart_left.dot(cart_right));
	float theta=acos(cart_left.dot(cart_right));

	//ROS_ERROR("theta:%f", theta);
	float l=(HEAD_SIZE/2)/tan(theta/2);
	return l;
}