bool Vector2::epsilonEquals(cocos2d::Vec2 other, float epsilon)
{
return epsilonEquals(other.x, other.y, epsilon);
}
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;
}
}
}
}
}