예제 #1
0
파일: Vector2.cpp 프로젝트: chakke/zzz
	bool Vector2::epsilonEquals(cocos2d::Vec2  other, float epsilon)
	{
		return epsilonEquals(other.x, other.y, epsilon);
	}
예제 #2
0
파일: Vector2.cpp 프로젝트: chakke/zzz
	bool Vector2::epsilonEquals(Vector2 * other, float epsilon)
	{
		return epsilonEquals(other->getX(), other->getY(), epsilon);
	}
void DotTracker::pointCorrespondenceCallback(const visp_camera_calibration::CalibPointArrayPtr& msg){
	
	ROS_INFO_STREAM("pointCorrespondenceCallback");
	points_.clear();
	trackers_.clear();
	
	
	std::string modelpoints__x_param(model_prefix_ + "/model_points_x");
	std::string modelpoints__y_param(model_prefix_ + "/model_points_y");
	std::string modelpoints__z_param(model_prefix_ + "/model_points_z");
	
	XmlRpc::XmlRpcValue modelpoints__x_list;
	XmlRpc::XmlRpcValue modelpoints__y_list;
	XmlRpc::XmlRpcValue modelpoints__z_list;
	ros::param::get(modelpoints__x_param,modelpoints__x_list);
	ros::param::get(modelpoints__y_param,modelpoints__y_list);
	ros::param::get(modelpoints__z_param,modelpoints__z_list);
	
	ROS_INFO_STREAM(modelpoints__x_param << " value " << modelpoints__x_list);
	ROS_INFO_STREAM(modelpoints__y_param << " value " << modelpoints__y_list);
	ROS_INFO_STREAM(modelpoints__z_param << " value " << modelpoints__z_list);
	
	ROS_ASSERT(modelpoints__x_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
	ROS_ASSERT(modelpoints__x_list.size() == modelpoints__y_list.size() && modelpoints__x_list.size()==modelpoints__z_list.size());
	for(int i=0;i<modelpoints__x_list.size();i++){
		ROS_ASSERT(modelpoints__x_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
		ROS_ASSERT(modelpoints__y_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
		ROS_ASSERT(modelpoints__z_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
				
		visp_camera_calibration::CalibPoint element = visp_camera_calibration::CalibPoint();
		
		
		element.X = modelpoints__x_list[i];
		element.Y = modelpoints__y_list[i];
		element.Z = modelpoints__z_list[i];
		
		ROS_INFO_STREAM("initialized"<< element.X << "," << element.Y << "," << element.Z); 
		
		points_.push_back(element);
		
		
		//ROS_INFO_STREAM("element i=" << element.i << " j=" << element.j);
		
		boost::shared_ptr<vpDot2> tracker(new vpDot2());
		
		tracker->setSizePrecision(.1);
		tracker->setEllipsoidShapePrecision(.8);
		tracker->setGrayLevelPrecision(.65);
		trackers_.push_back(tracker);
		trackers_status_.push_back(NOT_TRACKING);
	}
	
	//now we have all the trackers for each model point, let us initialate trackers based on 
	//suplied calibration data
	
	for(uint i=0;i<msg->points.size();i++){
		visp_camera_calibration::CalibPoint element = msg->points[i];
		
		ROS_INFO_STREAM("looking for i=" << element.i << " j=" << element.j << "," << element.X << "," << element.Y << "," << element.Z); 
		
		for(uint j=0;j<points_.size();j++){
			visp_camera_calibration::CalibPoint element2 = points_[j];
			
			if( epsilonEquals(element.X, element2.X, 0.001) && 	
				epsilonEquals(element.Y, element2.Y, 0.001) &&
				epsilonEquals(element.Z, element2.Z, 0.001)){
				
				//we found (epsilon) identical point
				ROS_INFO_STREAM("found i=" << element2.i << " j=" << element2.j << "," << element2.X << "," << element2.Y << "," << element2.Z); 
				try{
					trackers_[j]->initTracking(image_, vpImagePoint(element.i, element.j) , 10);//dodgy???
					trackers_status_[j] = TRACKING;
				}catch(...){
					ROS_ERROR_STREAM("failed to initialize dot " << j);
					trackers_status_[j] = NOT_TRACKING;
				}
			}
		}
	}
}