/**
* Constructor of the Monocular Pose Estimation Node class
*
*/
MPENode::MPENode(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private)
: nh_(nh), nh_private_(nh_private), have_camera_info_(false)
{
// Set up a dynamic reconfigure server.
// This should be done before reading parameter server values.
dynamic_reconfigure::Server<monocular_pose_estimator::MonocularPoseEstimatorConfig>::CallbackType cb_;
cb_ = boost::bind(&MPENode::dynamicParametersCallback, this, _1, _2);
dr_server_.setCallback(cb_);
// Initialize subscribers
image_sub_ = nh_.subscribe("/camera/image_raw", 1, &MPENode::imageCallback, this);
camera_info_sub_ = nh_.subscribe("/camera/camera_info", 1, &MPENode::cameraInfoCallback, this);
// Initialize pose publisher
pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("estimated_pose", 1);
// Initialize image publisher for visualization
image_transport::ImageTransport image_transport(nh_);
image_pub_ = image_transport.advertise("image_with_detections", 1);
// Create the marker positions from the test points
List4DPoints positions_of_markers_on_object;
// Read in the marker positions from the YAML parameter file
XmlRpc::XmlRpcValue points_list;
if (!nh_private_.getParam("marker_positions", points_list))
{
ROS_ERROR(
"%s: No reference file containing the marker positions, or the file is improperly formatted. Use the 'marker_positions_file' parameter in the launch file.",
ros::this_node::getName().c_str());
ros::shutdown();
}
else
{
positions_of_markers_on_object.resize(points_list.size());
for (int i = 0; i < points_list.size(); i++)
{
Eigen::Matrix<double, 4, 1> temp_point;
temp_point(0) = points_list[i]["x"];
temp_point(1) = points_list[i]["y"];
temp_point(2) = points_list[i]["z"];
temp_point(3) = 1;
positions_of_markers_on_object(i) = temp_point;
}
}
trackable_object_.setMarkerPositions(positions_of_markers_on_object);
ROS_INFO("The number of markers on the object are: %d", (int )positions_of_markers_on_object.size());
}
std::vector<cv::Point2f> edge::normalize(std::vector<T> cont){
std::vector<cv::Point2f> ret_contour;
cv::Point2d a(cont.front().x,cont.front().y);
cv::Point2d b(cont.back().x, cont.back().y);
//Calculating angle from vertical
b.x =b.x - a.x;
b.y =b.y - a.y;
double theta = std::acos(b.y/(cv::norm(b)));
if(b.x < 0) theta = -theta;
//Theta is the angle every point needs rotated.
//and -a is the translation
for(std::vector<cv::Point>::iterator i = cont.begin(); i!= cont.end(); i++ ){
//Apply translation
cv::Point2d temp_point(i->x-a.x,i->y-a.y);
//Apply roatation
double new_x= std::cos(theta) * temp_point.x - sin(theta)*temp_point.y;
double new_y = std::sin(theta) * temp_point.x + std::cos(theta) *temp_point.y;
ret_contour.push_back(cv::Point2f((float)new_x, (float)new_y));
}
return ret_contour;
}
void MeshDenoisingBase::updateVertexPosition(TriMesh &mesh, std::vector<TriMesh::Normal> &filtered_normals, int iteration_number, bool fixed_boundary)
{
std::vector<TriMesh::Point> new_points(mesh.n_vertices());
std::vector<TriMesh::Point> centroid;
for(int iter = 0; iter < iteration_number; iter++)
{
getFaceCentroid(mesh, centroid);
for(TriMesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); v_it++)
{
TriMesh::Point p = mesh.point(*v_it);
if(fixed_boundary && mesh.is_boundary(*v_it))
{
new_points.at(v_it->idx()) = p;
}
else
{
double face_num = 0.0;
TriMesh::Point temp_point(0.0, 0.0, 0.0);
for(TriMesh::VertexFaceIter vf_it = mesh.vf_iter(*v_it); vf_it.is_valid(); vf_it++)
{
TriMesh::Normal temp_normal = filtered_normals[vf_it->idx()];
TriMesh::Point temp_centroid = centroid[vf_it->idx()];
temp_point += temp_normal * (temp_normal | (temp_centroid - p));
face_num++;
}
p += temp_point / face_num;
new_points.at(v_it->idx()) = p;
}
}
for(TriMesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); v_it++)
mesh.set_point(*v_it, new_points[v_it->idx()]);
}
}
void check_for_pole(){
int num_scans = (int)((cur_scan.angle_max-cur_scan.angle_min)/cur_scan.angle_increment);
std::vector<std::vector<double> > point_vector;
std::vector<double> temp_point(2,0);
//loop through each scan
for(int i = 0; i < num_scans; i++){
if(cur_scan.ranges[i]==INFINITY ||
std::isnan(cur_scan.ranges[i])){
continue;
}
//ROS_INFO("looping through scans %d", i);
double current_range = cur_scan.ranges[i];
int good = 1;
int bad = 0;
int j = i+1;
while(1){
//check for end of array
if(j>=(num_scans-1)){
if(good >= POLE_HITS){
//ROS_INFO("pole hits: %d", good);
temp_point[0] = current_range;
temp_point[1] = cur_scan.angle_min + (cur_scan.angle_increment*((j+i)/2));
point_vector.push_back(temp_point);
}
break;
}else if(fabs(cur_scan.ranges[j]-current_range) > RANGE_THRESH){
bad++;
if(bad <= BAD_HITS){
continue;
}else{
if(good >= POLE_HITS){
temp_point[0] = current_range;
temp_point[1] = cur_scan.angle_min + (cur_scan.angle_increment*((j+i)/2));
point_vector.push_back(temp_point);
//ROS_INFO("pole hits: %d", good);
//ROS_INFO("Object Position: range->%f\tangle->%f", temp_point[0], temp_point[1]);
i = j;
}
break;
}
}else if(fabs(cur_scan.ranges[j]-current_range) < RANGE_THRESH){
good++;
}
j++;
}
}
//check each object to see if it's a pole
for(int i = 0; i < point_vector.size(); i++){
double range = point_vector[i][0];
double theta = point_vector[i][1];
double temp_x, temp_y;
//first add the laser offset
temp_x = cos(theta)*range + LASER_OFFSET;
temp_y = sin(theta)*range;
theta = atan2(temp_y, temp_x);
range = sqrt(pow(temp_x,2)+pow(temp_y,2));
if(checkBoundaries(range,theta)){
//publish
//geometry_msgs::Pose2D laserPoint;
geometry_msgs::PointStamped laserPoint;
laserPoint.point.x = cur_pos.x + range*cos(theta+cur_pos.theta);
laserPoint.point.y = cur_pos.y + range*sin(theta+cur_pos.theta);
//add to average and filter point
if(moving_avg_filter.size() < 10){
moving_avg_filter.push_back(laserPoint);
}else{
moving_avg_filter[filter_index%9] = laserPoint;
filter_index++;
}
laserPoint.point.x =0;
laserPoint.point.y =0;
for(int i = 0; i < moving_avg_filter.size(); i++){
laserPoint.point.x += moving_avg_filter[i].point.x;
laserPoint.point.y += moving_avg_filter[i].point.y;
}
laserPoint.point.x /= moving_avg_filter.size();
laserPoint.point.y /= moving_avg_filter.size();
pole_pub.publish(laserPoint);
break;
}
}
}
