void makeSendDataDetectedObj(vector<OBJPOS> car_position_vector,
vector<OBJPOS>::iterator cp_iterator,
LOCATION mloc,
ANGLE angle,
cv_tracker::obj_label& send_data)
{
geometry_msgs::Point tmpPoint;
for(uint i=0; i<car_position_vector.size() ; i++, cp_iterator++){
//middle of right-lower and left-upper
double U = cp_iterator->x1 + cp_iterator->x2/2;
double V = cp_iterator->y1 + cp_iterator->y2/2;
//convert from "image" coordinate system to "camera" coordinate system
ol.setOriginalValue(U,V,cp_iterator->distance);
LOCATION ress = ol.cal();
/* convert from "camera" coordinate system to "map" coordinate system */
tf::Vector3 pos_in_camera_coord(ress.X, ress.Y, ress.Z);
tf::Vector3 converted = transformCam2Map * pos_in_camera_coord;
tmpPoint.x = converted.x();
tmpPoint.y = converted.y();
tmpPoint.z = converted.z();
send_data.reprojected_pos.push_back(tmpPoint);
}
}
static void camera_info_callback(const sensor_msgs::CameraInfo& msg)
{
double fkx = msg.K[0 * 3 + 0]; // get K[0][0]
double fky = msg.K[1 * 3 + 1]; // get K[1][1]
double Ox = msg.K[0 * 3 + 2]; // get K[0][2]
double Oy = msg.K[1 * 3 + 2]; // get K[1][2]
ol.setCameraParam(fkx,fky,Ox,Oy);
}
void makeSendDataDetectedObj(vector<OBJPOS> car_position_vector,
vector<OBJPOS>::iterator cp_iterator,
autoware_msgs::obj_label& send_data)
{
geometry_msgs::Point tmpPoint;
for(uint i=0; i<car_position_vector.size() ; i++, cp_iterator++){
//middle of right-lower and left-upper
double U = cp_iterator->x1 + cp_iterator->x2/2;
double V = cp_iterator->y1 + cp_iterator->y2/2;
//convert from "image" coordinate system to "camera" coordinate system
ol.setOriginalValue(U,V,cp_iterator->distance);
LOCATION ress = ol.cal();
/* convert from "camera" coordinate system to "map" coordinate system */
tf::Vector3 pos_in_camera_coord(ress.X, ress.Y, ress.Z);
static tf::TransformListener listener;
try {
listener.lookupTransform("map", camera_id_str, ros::Time(0), transformCam2Map);
}
catch (tf::TransformException ex) {
ROS_INFO("%s", ex.what());
return;
}
tf::Vector3 converted = transformCam2Map * pos_in_camera_coord;
tmpPoint.x = converted.x();
tmpPoint.y = converted.y();
tmpPoint.z = converted.z();
send_data.reprojected_pos.push_back(tmpPoint);
send_data.obj_id.push_back(cp_iterator->id);
}
}
