void TransformROSBridge::calculateTwist(const tf::Transform& _current_trans, const tf::Transform& _prev_trans, tf::Vector3& _linear_twist, tf::Vector3& _angular_twist, double _dt) { // current rotation matrix tf::Matrix3x3 current_basis = _current_trans.getBasis(); // linear twist tf::Vector3 current_origin = _current_trans.getOrigin(); tf::Vector3 prev_origin = _prev_trans.getOrigin(); _linear_twist = current_basis.transpose() * ((current_origin - prev_origin) / _dt); // angular twist // R = exp(omega_w*dt) * prev_R // omega_w is described in global coordinates in relationships of twist transformation. // it is easier to calculate omega using hrp functions than tf functions tf::Matrix3x3 prev_basis = _prev_trans.getBasis(); double current_rpy[3], prev_rpy[3]; current_basis.getRPY(current_rpy[0], current_rpy[1], current_rpy[2]); prev_basis.getRPY(prev_rpy[0], prev_rpy[1], prev_rpy[2]); hrp::Matrix33 current_hrpR = hrp::rotFromRpy(current_rpy[0], current_rpy[1], current_rpy[2]); hrp::Matrix33 prev_hrpR = hrp::rotFromRpy(prev_rpy[0], prev_rpy[1], prev_rpy[2]); hrp::Vector3 hrp_omega = current_hrpR.transpose() * hrp::omegaFromRot(current_hrpR * prev_hrpR.transpose()) / _dt; _angular_twist.setX(hrp_omega[0]); _angular_twist.setY(hrp_omega[1]); _angular_twist.setZ(hrp_omega[2]); return; }
tf::Vector3 JPCT_Math::rotate(tf::Vector3 vec, tf::Matrix3x3& mat) { float xr = vec.getX() * mat[0][0] + vec.getY() * mat[1][0] + vec.getZ() * mat[2][0]; float yr = vec.getX() * mat[0][1] + vec.getY() * mat[1][1] + vec.getZ() * mat[2][1]; float zr = vec.getX() * mat[0][2] + vec.getY() * mat[1][2] + vec.getZ() * mat[2][2]; vec.setX(xr); vec.setY(yr); vec.setZ(zr); return vec; }
void BeaconKFNode::initializeRos( void ) { ros::NodeHandle private_nh("~"); ros::NodeHandle nh(""); private_nh.param("world_fixed_frame", _world_fixed_frame, std::string("map")); private_nh.param("odometry_frame", _odometry_frame, std::string("odom")); private_nh.param("platform_frame", _platform_frame, std::string("platform")); //setup platform offset position and angle int platform_index; std::vector<double> empty_vec; private_nh.param("platform_index", platform_index, 0); private_nh.param("platform_x_coords", _platform_x_coords, empty_vec); private_nh.param("platform_y_coords", _platform_y_coords, empty_vec); double reference_x; double reference_y; private_nh.param("reference_x", reference_x, 1.0); private_nh.param("reference_y", reference_y, 0.0); _reference_coords.push_back(reference_x); _reference_coords.push_back(reference_y); double platform_angle; private_nh.param("platform_orientation", platform_angle, 0.0); _platform_origin.setX(_platform_x_coords[platform_index]); _platform_origin.setY(_platform_y_coords[platform_index]); _platform_origin.setZ(0); platform_angle = platform_angle*(M_PI/180); platform_angle = platform_angle + atan(_reference_coords[1]/_reference_coords[0]); _platform_orientation = tf::createQuaternionFromYaw(platform_angle); //beacon message subscriber callback _beacon_sub = nh.subscribe( "beacon_pose", 1, &BeaconKFNode::beaconCallback, this); //define timer rates double update_period; double broadcast_period; private_nh.param("update_period", update_period, 1.0); private_nh.param("broadcast_period", broadcast_period, 0.05); //timer for broadcasting the map correction xfrom _transform_broadcast_timer = private_nh.createTimer(ros::Duration(broadcast_period), &BeaconKFNode::transformBroadcastCallback, this); //timer for updating the EKF _update_timer = private_nh.createTimer(ros::Duration(update_period), &BeaconKFNode::filterUpdateCallback, this); //set dynamic reconfigure callback dr_server.setCallback(boost::bind(&BeaconKFNode::configCallback, this, _1, _2)); }
/* Dynamic reconfigure callback */ void BeaconKFNode::configCallback(beacon_localizer::beacon_localizer_paramsConfig &config, uint32_t level) { double platform_angle; int platform_index; _reference_coords[0] = config.reference_x; _reference_coords[1] = config.reference_y; //get platform orientation in degrees, convert to reference system platform_angle = config.platform_orientation; platform_angle = platform_angle*(M_PI/180); platform_angle = platform_angle + atan(_reference_coords[1]/_reference_coords[0]); _platform_orientation = tf::createQuaternionFromYaw(platform_angle); platform_index = config.platform_index; _platform_origin.setX(_platform_x_coords[platform_index]); _platform_origin.setY(_platform_y_coords[platform_index]); }
bool checkCloud(const sensor_msgs::PointCloud2& cloud_msg, typename pcl::PointCloud<T>::Ptr hand_cloud, //typename pcl::PointCloud<T>::Ptr finger_cloud, const std::string& frame_id, tf::Vector3& hand_position, tf::Vector3& arm_position, tf::Vector3& arm_direction) { typename pcl::PointCloud<T>::Ptr cloud(new pcl::PointCloud<T>); pcl::fromROSMsg(cloud_msg, *cloud); if((cloud->points.size() < g_config.min_cluster_size) || (cloud->points.size() > g_config.max_cluster_size)) return false; pcl::PCA<T> pca; pca.setInputCloud(cloud); Eigen::Vector4f mean = pca.getMean(); if((mean.coeff(0) < g_config.min_x) || (mean.coeff(0) > g_config.max_x)) return false; if((mean.coeff(1) < g_config.min_y) || (mean.coeff(1) > g_config.max_y)) return false; if((mean.coeff(2) < g_config.min_z) || (mean.coeff(2) > g_config.max_z)) return false; Eigen::Vector3f eigen_value = pca.getEigenValues(); double ratio = eigen_value.coeff(0) / eigen_value.coeff(1); if((ratio < g_config.min_eigen_value_ratio) || (ratio > g_config.max_eigen_value_ratio)) return false; T search_point; Eigen::Matrix3f ev = pca.getEigenVectors(); Eigen::Vector3f main_axis(ev.coeff(0, 0), ev.coeff(1, 0), ev.coeff(2, 0)); main_axis.normalize(); arm_direction.setX(main_axis.coeff(0)); arm_direction.setY(main_axis.coeff(1)); arm_direction.setZ(main_axis.coeff(2)); arm_position.setX(mean.coeff(0)); arm_position.setY(mean.coeff(1)); arm_position.setZ(mean.coeff(2)); main_axis = (-main_axis * 0.3) + Eigen::Vector3f(mean.coeff(0), mean.coeff(1), mean.coeff(2)); search_point.x = main_axis.coeff(0); search_point.y = main_axis.coeff(1); search_point.z = main_axis.coeff(2); //find hand pcl::KdTreeFLANN<T> kdtree; kdtree.setInputCloud(cloud); //find the closet point from the serach_point std::vector<int> point_indeices(1); std::vector<float> point_distances(1); if ( kdtree.nearestKSearch (search_point, 1, point_indeices, point_distances) > 0 ) { //update search point search_point = cloud->points[point_indeices[0]]; //show seach point if(g_marker_array_pub.getNumSubscribers() != 0) { pushSimpleMarker(search_point.x, search_point.y, search_point.z, 1.0, 0, 0, 0.02, g_marker_id, g_marker_array, frame_id); } //hand point_indeices.clear(); point_distances.clear(); kdtree.radiusSearch(search_point, g_config.hand_lenght, point_indeices, point_distances); for (size_t i = 0; i < point_indeices.size (); ++i) { hand_cloud->points.push_back(cloud->points[point_indeices[i]]); hand_cloud->points.back().r = 255; hand_cloud->points.back().g = 0; hand_cloud->points.back().b = 0; } Eigen::Vector4f centroid; pcl::compute3DCentroid(*hand_cloud, centroid); if(g_marker_array_pub.getNumSubscribers() != 0) { pushSimpleMarker(centroid.coeff(0), centroid.coeff(1), centroid.coeff(2), 0.0, 1.0, 0, 0.02, g_marker_id, g_marker_array, frame_id); } hand_position.setX(centroid.coeff(0)); hand_position.setY(centroid.coeff(1)); hand_position.setZ(centroid.coeff(2)); #if 0 //fingers search_point.x = centroid.coeff(0); search_point.y = centroid.coeff(1); search_point.z = centroid.coeff(2); std::vector<int> point_indeices_inner; std::vector<float> point_distances_inner; kdtree.radiusSearch(search_point, 0.07, point_indeices_inner, point_distances_inner); std::vector<int> point_indeices_outter; std::vector<float> point_distances_outter; kdtree.radiusSearch(search_point, 0.1, point_indeices_outter, point_distances_outter); //ROS_INFO("before %d %d", point_indeices_inner.size(), point_indeices_outter.size()); std::vector<int>::iterator it; for(size_t i = 0; i < point_indeices_inner.size(); i++) { it = std::find(point_indeices_outter.begin(), point_indeices_outter.end(), point_indeices_inner[i]); if(it != point_indeices_outter.end()) { point_indeices_outter.erase(it); } } //ROS_INFO("after %d %d", point_indeices_inner.size(), point_indeices_outter.size()); //ROS_DEBUG_THROTTLE(1.0, "found %lu", point_indeices.size ()); for (size_t i = 0; i < point_indeices_outter.size (); ++i) { finger_cloud->points.push_back(cloud->points[point_indeices_outter[i]]); finger_cloud->points.back().r = 255; finger_cloud->points.back().g = 0; finger_cloud->points.back().b = 0; } #endif } else { return false; } if(g_marker_array_pub.getNumSubscribers() != 0) { pushEigenMarker<T>(pca, g_marker_id, g_marker_array, 0.1, frame_id); } return true; }
void aero_path_planning::pointToVector(const aero_path_planning::Point& point, tf::Vector3& vector) { vector.setX(point.x); vector.setY(point.y); vector.setZ(point.z); }
// callback for the complete message void complete_message_callback(const homog_track::HomogComplete& msg) { /********** Begin splitting up the incoming message *********/ // getting boolean indicating the reference has been set reference_set = msg.reference_set; // if the reference is set then will break out the points if (reference_set) { // initializer temp scalar to zero temp_scalar = cv::Mat::zeros(1,1,CV_64F); // getting the current marker points circles_curr = msg.current_points; // getting the refernce marker points circles_ref = msg.reference_points; // setting the current points to the point vector curr_red_p.x = circles_curr.red_circle.x; curr_green_p.x = circles_curr.green_circle.x; curr_cyan_p.x = circles_curr.cyan_circle.x; curr_purple_p.x = circles_curr.purple_circle.x; curr_red_p.y = circles_curr.red_circle.y; curr_green_p.y = circles_curr.green_circle.y; curr_cyan_p.y = circles_curr.cyan_circle.y; curr_purple_p.y = circles_curr.purple_circle.y; curr_points_p.push_back(curr_red_p); curr_points_p.push_back(curr_green_p); curr_points_p.push_back(curr_cyan_p); curr_points_p.push_back(curr_purple_p); // converting the points to be the projective coordinates for (int ii = 0; ii < curr_points_m.size(); ii++) { curr_points_m[ii] = K.inv(cv::DECOMP_LU)*curr_points_m[ii]; std::cout << "currpoints at " << ii << " is: " << curr_points_m[ii] << std::endl; } // setting the reference points to the point vector ref_red_p.x = circles_ref.red_circle.x; ref_green_p.x = circles_ref.green_circle.x; ref_cyan_p.x = circles_ref.cyan_circle.x; ref_purple_p.x = circles_ref.purple_circle.x; ref_red_p.y = circles_ref.red_circle.y; ref_green_p.y = circles_ref.green_circle.y; ref_cyan_p.y = circles_ref.cyan_circle.y; ref_purple_p.y = circles_ref.purple_circle.y; ref_points_p.push_back(ref_red_p); ref_points_p.push_back(ref_green_p); ref_points_p.push_back(ref_cyan_p); ref_points_p.push_back(ref_purple_p); // setting the reference points to the matrix vector, dont need to do the last one because its already 1 ref_red_m.at<double>(0,0) = ref_red_p.x; ref_red_m.at<double>(1,0) = ref_red_p.y; ref_green_m.at<double>(0,0) = ref_green_p.x; ref_green_m.at<double>(1,0) = ref_green_p.y; ref_cyan_m.at<double>(0,0) = ref_cyan_p.x; ref_cyan_m.at<double>(1,0) = ref_cyan_p.y; ref_purple_m.at<double>(0,0) = ref_purple_p.x; ref_purple_m.at<double>(1,0) = ref_purple_p.y; ref_points_m.push_back(ref_red_m); ref_points_m.push_back(ref_green_m); ref_points_m.push_back(ref_cyan_m); ref_points_m.push_back(ref_purple_m); // converting the points to be the projective coordinates for (int ii = 0; ii < ref_points_m.size(); ii++) { ref_points_m[ii] = K.inv(cv::DECOMP_LU)*ref_points_m[ii]; //std::cout << "refpoints at " << ii << " is: " << ref_points_m[ii] << std::endl; } // if any of the points have a -1 will skip over the homography if (curr_red_p.x != -1 && curr_green_p.x != -1 && curr_cyan_p.x != -1 && curr_purple_p.x != -1) { //std::cout << "hi" << std::endl; // finding the perspective homography G = cv::findHomography(curr_points_p,ref_points_p,0); //G = cv::findHomography(ref_points_p,ref_points_p,0); std::cout << "G: " << G << std::endl; // decomposing the homography into the four solutions // G and K are 3x3 // R is 3x3 // 3x1 // 3x1 // successful_decomp is the number of solutions found successful_decomp = cv::decomposeHomographyMat(G,K,R,T,n); std::cout << "successful_decomp: " << successful_decomp << std::endl; // if the decomp is successful will find the best matching if (successful_decomp > 0) { std::cout << std::endl << std::endl << " begin check for visibility" << std::endl; // finding the alphas alpha_red.data = 1/(G.at<double>(2,0)*ref_red_p.x + G.at<double>(2,1)*ref_red_p.y + 1); alpha_green.data = 1/(G.at<double>(2,0)*ref_green_p.x + G.at<double>(2,1)*ref_green_p.y + 1); alpha_cyan.data = 1/(G.at<double>(2,0)*ref_cyan_p.x + G.at<double>(2,1)*ref_cyan_p.y + 1); alpha_purple.data = 1/(G.at<double>(2,0)*ref_purple_p.x + G.at<double>(2,1)*ref_purple_p.y + 1); // finding the solutions that give the positive results for (int ii = 0; ii < successful_decomp; ii++) { std::cout << "solution set number " << ii << std::endl; // performing the operation transpose(m)*R*n to check if greater than 0 later // order operating on is red green cyan purple for (int jj = 0; jj < 4; jj++) { //std::cout << " T size: " << T[ii].size() << std::endl; //std::cout << " T type: " << T[ii].type() << std::endl; std::cout << " T value: " << T[ii] << std::endl; //std::cout << " temp scalar 1 " << std::endl; //std::cout << " temp scalar size: " << temp_scalar.size() << std::endl; //std::cout << " temp scalar type: " << temp_scalar.type() << std::endl; //std::cout << " temp scalar value " << temp_scalar <<std::endl; temp_scalar = curr_points_m[jj].t(); //std::cout << " temp scalar 2 " << std::endl; //std::cout << " temp scalar size: " << temp_scalar.size() << std::endl; //std::cout << " temp scalar type: " << temp_scalar.type() << std::endl; //std::cout << " temp scalar value " << temp_scalar <<std::endl; //std::cout << " R size: " << R[ii].size() << std::endl; //std::cout << " R type: " << R[ii].type() << std::endl; //std::cout << " R value: " << R[ii] << std::endl; temp_scalar = temp_scalar*R[ii]; //std::cout << " temp scalar 3 " << std::endl; //std::cout << " temp scalar size: " << temp_scalar.size() << std::endl; //std::cout << " temp scalar type: " << temp_scalar.type() << std::endl; //std::cout << " temp scalar value " << temp_scalar <<std::endl; //std::cout << " n size: " << n[ii].size() << std::endl; //std::cout << " n type: " << n[ii].type() << std::endl; std::cout << " n value: " << n[ii] << std::endl; temp_scalar = temp_scalar*n[ii]; //std::cout << " temp scalar size: " << temp_scalar.size() << std::endl; //std::cout << " temp scalar type: " << temp_scalar.type() << std::endl; //std::cout << " temp scalar value " << temp_scalar <<std::endl; //std::cout << " temp scalar value at 0,0" << temp_scalar.at<double>(0,0) << std::endl; scalar_value_check.push_back(temp_scalar.at<double>(0,0)); ////std::cout << " scalar value check size: " << scalar_value_check.size() << std::endl; //std::cout << " \tthe value for the " << jj << " visibility check is: " << scalar_value_check[4*ii+jj] << std::endl; } } std::cout << " end check for visibility" << std::endl << std::endl; // restting first solution found and second solution found first_solution_found = false; second_solution_found = false; fc_found = false; // getting the two solutions or only one if there are not two for (int ii = 0; ii < successful_decomp; ii++) { // getting the values onto the temporary vector // getting the start and end of the next solution temp_solution_start = scalar_value_check.begin() + 4*ii; temp_solution_end = scalar_value_check.begin() + 4*ii+4; temp_solution.assign(temp_solution_start,temp_solution_end); // checking if all the values are positive all_positive = true; current_temp_index = 0; while (all_positive && current_temp_index < 4) { if (temp_solution[current_temp_index] >= 0) { current_temp_index++; } else { all_positive = false; } } // if all the values were positive and a first solution has not been found will assign // to first solution. if all positive and first solution has been found will assign // to second solution. if all positive is false then will not do anything if (all_positive && first_solution_found && !second_solution_found) { // setting it to indicate a solution has been found second_solution_found = true; // setting the rotation, translation, and normal to be the second set second_R = R[ii]; second_T = T[ii]; second_n = n[ii]; // setting the projected values second_solution = temp_solution; } else if (all_positive && !first_solution_found) { // setting it to indicate a solution has been found first_solution_found = true; // setting the rotation, translation, and normal to be the first set first_R = R[ii]; first_T = T[ii]; first_n = n[ii]; // setting the projected values first_solution = temp_solution; } // erasing all the values from the temp solution temp_solution.erase(temp_solution.begin(),temp_solution.end()); } // erasing all the scalar values from the check scalar_value_check.erase(scalar_value_check.begin(),scalar_value_check.end()); // displaying the first solution if it was found if (first_solution_found) { std::cout << std::endl << "first R: " << first_R << std::endl; std::cout << "first T: " << first_T << std::endl; std::cout << "first n: " << first_n << std::endl; for (double ii : first_solution) { std::cout << ii << " "; } std::cout << std::endl; } // displaying the second solution if it was found if (second_solution_found) { std::cout << std::endl << "second R: " << second_R << std::endl; std::cout << "second T: " << second_T << std::endl; std::cout << "second n: " << second_n << std::endl; for (double ii : second_solution) { std::cout << ii << " "; } std::cout << std::endl; } // because the reference is set to the exact value when when n should have only a z componenet, the correct // choice should be the one closest to n_ref = [0,0,1]^T which will be the one with the greatest dot product with n_ref if (first_solution_found && second_solution_found) { if (first_n.dot(n_ref) >= second_n.dot(n_ref)) { R_fc = first_R; T_fc = first_T; } else { R_fc = second_R; T_fc = second_T; } fc_found = true; } else if(first_solution_found) { R_fc = first_R; T_fc = first_T; fc_found = true; } //if a solution was found will publish // need to convert to pose message so use if (fc_found) { // converting the rotation from a cv matrix to quaternion, first need it as a matrix3x3 R_fc_tf[0][0] = R_fc.at<double>(0,0); R_fc_tf[0][1] = R_fc.at<double>(0,1); R_fc_tf[0][2] = R_fc.at<double>(0,2); R_fc_tf[1][0] = R_fc.at<double>(1,0); R_fc_tf[1][1] = R_fc.at<double>(1,1); R_fc_tf[1][2] = R_fc.at<double>(1,2); R_fc_tf[2][0] = R_fc.at<double>(2,0); R_fc_tf[2][1] = R_fc.at<double>(2,1); R_fc_tf[2][2] = R_fc.at<double>(2,2); std::cout << "Final R:\n" << R_fc << std::endl; // converting the translation to a vector 3 T_fc_tf.setX(T_fc.at<double>(0,0)); T_fc_tf.setY(T_fc.at<double>(0,1)); T_fc_tf.setZ(T_fc.at<double>(0,2)); std::cout << "Final T :\n" << T_fc << std::endl; // getting the rotation as a quaternion R_fc_tf.getRotation(Q_fc_tf); std::cout << "current orientation:" << "\n\tx:\t" << Q_fc_tf.getX() << "\n\ty:\t" << Q_fc_tf.getY() << "\n\tz:\t" << Q_fc_tf.getZ() << "\n\tw:\t" << Q_fc_tf.getW() << std::endl; std::cout << "norm of quaternion:\t" << Q_fc_tf.length() << std::endl; // getting the negated version of the quaternion for the check Q_fc_tf_negated = tf::Quaternion(-Q_fc_tf.getX(),-Q_fc_tf.getY(),-Q_fc_tf.getZ(),-Q_fc_tf.getW()); std::cout << "negated orientation:" << "\n\tx:\t" << Q_fc_tf_negated.getX() << "\n\ty:\t" << Q_fc_tf_negated.getY() << "\n\tz:\t" << Q_fc_tf_negated.getZ() << "\n\tw:\t" << Q_fc_tf_negated.getW() << std::endl; std::cout << "norm of negated quaternion:\t" << Q_fc_tf_negated.length() << std::endl; // showing the last orientation std::cout << "last orientation:" << "\n\tx:\t" << Q_fc_tf_last.getX() << "\n\ty:\t" << Q_fc_tf_last.getY() << "\n\tz:\t" << Q_fc_tf_last.getZ() << "\n\tw:\t" << Q_fc_tf_last.getW() << std::endl; std::cout << "norm of last quaternion:\t" << Q_fc_tf_last.length() << std::endl; // checking if the quaternion has flipped Q_norm_current_diff = std::sqrt(std::pow(Q_fc_tf.getX() - Q_fc_tf_last.getX(),2.0) + std::pow(Q_fc_tf.getY() - Q_fc_tf_last.getY(),2.0) + std::pow(Q_fc_tf.getZ() - Q_fc_tf_last.getZ(),2.0) + std::pow(Q_fc_tf.getW() - Q_fc_tf_last.getW(),2.0)); std::cout << "current difference:\t" << Q_norm_current_diff << std::endl; Q_norm_negated_diff = std::sqrt(std::pow(Q_fc_tf_negated.getX() - Q_fc_tf_last.getX(),2.0) + std::pow(Q_fc_tf_negated.getY() - Q_fc_tf_last.getY(),2.0) + std::pow(Q_fc_tf_negated.getZ() - Q_fc_tf_last.getZ(),2.0) + std::pow(Q_fc_tf_negated.getW() - Q_fc_tf_last.getW(),2.0)); std::cout << "negated difference:\t" << Q_norm_negated_diff << std::endl; if (Q_norm_current_diff > Q_norm_negated_diff) { Q_fc_tf = Q_fc_tf_negated; } // updating the last Q_fc_tf_last = Q_fc_tf; // converting the tf quaternion to a geometry message quaternion Q_fc_gm.x = Q_fc_tf.getX(); Q_fc_gm.y = Q_fc_tf.getY(); Q_fc_gm.z = Q_fc_tf.getZ(); Q_fc_gm.w = Q_fc_tf.getW(); // converting the tf vector3 to a point P_fc_gm.x = T_fc_tf.getX(); P_fc_gm.y = T_fc_tf.getY(); P_fc_gm.z = T_fc_tf.getZ(); // setting the transform with the values fc_tf.setOrigin(T_fc_tf); fc_tf.setRotation(Q_fc_tf); tf_broad.sendTransform(tf::StampedTransform(fc_tf, msg.header.stamp,"f_star","f_current")); // setting the decomposed message pose_fc_gm.position = P_fc_gm; pose_fc_gm.orientation = Q_fc_gm; decomposed_msg.pose = pose_fc_gm; decomposed_msg.header.stamp = msg.header.stamp; decomposed_msg.header.frame_id = "current_frame_normalized"; decomposed_msg.alpha_red = alpha_red; decomposed_msg.alpha_green = alpha_green; decomposed_msg.alpha_cyan = alpha_cyan; decomposed_msg.alpha_purple = alpha_purple; homog_decomp_pub.publish(decomposed_msg); std::cout << "complete message\n" << decomposed_msg << std::endl << std::endl; // publish the marker marker.pose = pose_fc_gm; marker_pub.publish(marker); } } } // erasing all the temporary points if (first_solution_found || second_solution_found) { // erasing all the point vectors and matrix vectors curr_points_p.erase(curr_points_p.begin(),curr_points_p.end()); ref_points_p.erase(ref_points_p.begin(),ref_points_p.end()); curr_points_m.erase(curr_points_m.begin(),curr_points_m.end()); ref_points_m.erase(ref_points_m.begin(),ref_points_m.end()); } } /********** End splitting up the incoming message *********/ }