// Go to this pose
bool go_home(tf::Pose& pose_) {
tf::Vector3 start_p(pose_.getOrigin());
tf::Quaternion start_o(pose_.getRotation());
msg_pose.pose.position.x = start_p.x();
msg_pose.pose.position.y = start_p.y();
msg_pose.pose.position.z = start_p.z();
msg_pose.pose.orientation.w = start_o.w();
msg_pose.pose.orientation.x = start_o.x();
msg_pose.pose.orientation.y = start_o.y();
msg_pose.pose.orientation.z = start_o.z();
pub_.publish(msg_pose);
sendNormalForce(0);
ros::Rate thread_rate(2);
ROS_INFO("Homing...");
while(ros::ok()) {
double oerr =(ee_pose.getRotation() - start_o).length() ;
double perr = (ee_pose.getOrigin() - start_p).length();
feedback_.progress = 0.5*(perr+oerr);
as_.publishFeedback(feedback_);
ROS_INFO_STREAM("Error: "<<perr<<", "<<oerr);
if(perr< 0.02 && oerr < 0.02) {
break;
}
if (as_.isPreemptRequested() || !ros::ok() )
{
sendNormalForce(0);
sendPose(ee_pose);
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
return false;
}
thread_rate.sleep();
}
return ros::ok();
}
double score_segmentation( vector<int> &segmentation, string& outstring )
{
// Score Heuristics:
// No need to use Viterbi to know a given segmentation is bad
// e.g.: in some cases we discard a segmentation because it includes a very large character
// in other cases we do it because the overlapping between two chars is too large
// TODO Add more heuristics (e.g. penalize large inter-character variance)
Mat interdist ((int)segmentation.size()-1, 1, CV_32F, 1);
for (size_t i=0; i<segmentation.size()-1; i++)
{
interdist.at<float>((int)i,0) = (float)oversegmentation[segmentation[(int)i+1]]*step_size
- (float)oversegmentation[segmentation[(int)i]]*step_size;
if ((float)interdist.at<float>((int)i,0)/win_size > 2.25) // TODO explain how did you set this thrs
{
return -DBL_MAX;
}
if ((float)interdist.at<float>((int)i,0)/win_size < 0.15) // TODO explain how did you set this thrs
{
return -DBL_MAX;
}
}
Scalar m, std;
meanStdDev(interdist, m, std);
//double interdist_std = std[0];
//TODO Extracting start probs from lexicon (if we have it) may boost accuracy!
vector<double> start_p(vocabulary.size());
for (int i=0; i<(int)vocabulary.size(); i++)
start_p[i] = log(1.0/vocabulary.size());
Mat V = Mat::ones((int)segmentation.size(),(int)vocabulary.size(),CV_64FC1);
V = V * -DBL_MAX;
vector<string> path(vocabulary.size());
// Initialize base cases (t == 0)
for (int i=0; i<(int)vocabulary.size(); i++)
{
V.at<double>(0,i) = start_p[i] + recognition_probabilities[segmentation[0]][i];
path[i] = vocabulary.at(i);
}
// Run Viterbi for t > 0
for (int t=1; t<(int)segmentation.size(); t++)
{
vector<string> newpath(vocabulary.size());
for (int i=0; i<(int)vocabulary.size(); i++)
{
double max_prob = -DBL_MAX;
int best_idx = 0;
for (int j=0; j<(int)vocabulary.size(); j++)
{
double prob = V.at<double>(t-1,j) + transition_p.at<double>(j,i) + recognition_probabilities[segmentation[t]][i];
if ( prob > max_prob)
{
max_prob = prob;
best_idx = j;
}
}
V.at<double>(t,i) = max_prob;
newpath[i] = path[best_idx] + vocabulary.at(i);
}
// Don't need to remember the old paths
path.swap(newpath);
}
double max_prob = -DBL_MAX;
int best_idx = 0;
for (int i=0; i<(int)vocabulary.size(); i++)
{
double prob = V.at<double>((int)segmentation.size()-1,i);
if ( prob > max_prob)
{
max_prob = prob;
best_idx = i;
}
}
outstring = path[best_idx];
return (max_prob / (segmentation.size()-1));
}
void run( Mat& image,
string& out_sequence,
vector<Rect>* component_rects,
vector<string>* component_texts,
vector<float>* component_confidences,
int component_level)
{
CV_Assert( (image.type() == CV_8UC1) || (image.type() == CV_8UC3) );
CV_Assert( (image.cols > 0) && (image.rows > 0) );
CV_Assert( component_level == OCR_LEVEL_WORD );
out_sequence.clear();
if (component_rects != NULL)
component_rects->clear();
if (component_texts != NULL)
component_texts->clear();
if (component_confidences != NULL)
component_confidences->clear();
// First we split a line into words
vector<Mat> words_mask;
vector<Rect> words_rect;
/// Find contours
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
Mat tmp;
image.copyTo(tmp);
findContours( tmp, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE, Point(0, 0) );
if (contours.size() < 6)
{
//do not split lines with less than 6 characters
words_mask.push_back(image);
words_rect.push_back(Rect(0,0,image.cols,image.rows));
}
else
{
Mat_<float> vector_w((int)image.cols,1);
reduce(image, vector_w, 0, REDUCE_SUM, -1);
vector<int> spaces;
vector<int> spaces_start;
vector<int> spaces_end;
int space_count=0;
int last_one_idx;
int s_init = 0, s_end=vector_w.cols;
for (int s=0; s<vector_w.cols; s++)
{
if (vector_w.at<float>(0,s) == 0)
s_init = s+1;
else
break;
}
for (int s=vector_w.cols-1; s>=0; s--)
{
if (vector_w.at<float>(0,s) == 0)
s_end = s;
else
break;
}
for (int s=s_init; s<s_end; s++)
{
if (vector_w.at<float>(0,s) == 0)
{
space_count++;
} else {
if (space_count!=0)
{
spaces.push_back(space_count);
spaces_start.push_back(last_one_idx);
spaces_end.push_back(s-1);
}
space_count = 0;
last_one_idx = s;
}
}
Scalar mean_space,std_space;
meanStdDev(Mat(spaces),mean_space,std_space);
int num_word_spaces = 0;
int last_word_space_end = 0;
for (int s=0; s<(int)spaces.size(); s++)
{
if (spaces_end.at(s)-spaces_start.at(s) > mean_space[0]+(mean_space[0]*1.1)) //this 1.1 is a param?
{
if (num_word_spaces == 0)
{
//cout << " we have a word from 0 to " << spaces_start.at(s) << endl;
Mat word_mask;
Rect word_rect = Rect(0,0,spaces_start.at(s),image.rows);
image(word_rect).copyTo(word_mask);
words_mask.push_back(word_mask);
words_rect.push_back(word_rect);
}
else
{
//cout << " we have a word from " << last_word_space_end << " to " << spaces_start.at(s) << endl;
Mat word_mask;
Rect word_rect = Rect(last_word_space_end,0,spaces_start.at(s)-last_word_space_end,image.rows);
image(word_rect).copyTo(word_mask);
words_mask.push_back(word_mask);
words_rect.push_back(word_rect);
}
num_word_spaces++;
last_word_space_end = spaces_end.at(s);
}
}
//cout << " we have a word from " << last_word_space_end << " to " << vector_w.cols << endl << endl << endl;
Mat word_mask;
Rect word_rect = Rect(last_word_space_end,0,vector_w.cols-last_word_space_end,image.rows);
image(word_rect).copyTo(word_mask);
words_mask.push_back(word_mask);
words_rect.push_back(word_rect);
}
for (int w=0; w<(int)words_mask.size(); w++)
{
vector< vector<int> > observations;
vector< vector<double> > confidences;
vector<int> obs;
// First find contours and sort by x coordinate of bbox
words_mask[w].copyTo(tmp);
if (tmp.empty())
continue;
contours.clear();
hierarchy.clear();
/// Find contours
findContours( tmp, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE, Point(0, 0) );
vector<Rect> contours_rect;
for (int i=0; i<(int)contours.size(); i++)
{
contours_rect.push_back(boundingRect(contours[i]));
}
sort(contours_rect.begin(), contours_rect.end(), sort_rect_horiz);
// Do character recognition foreach contour
for (int i=0; i<(int)contours.size(); i++)
{
Mat tmp_mask;
words_mask[w](contours_rect.at(i)).copyTo(tmp_mask);
vector<int> out_class;
vector<double> out_conf;
classifier->eval(tmp_mask,out_class,out_conf);
if (!out_class.empty())
obs.push_back(out_class[0]);
observations.push_back(out_class);
confidences.push_back(out_conf);
}
//This must be extracted from dictionary, or just assumed to be equal for all characters
vector<double> start_p(vocabulary.size());
for (int i=0; i<(int)vocabulary.size(); i++)
start_p[i] = 1.0/vocabulary.size();
Mat V = Mat::zeros((int)observations.size(),(int)vocabulary.size(),CV_64FC1);
vector<string> path(vocabulary.size());
// Initialize base cases (t == 0)
for (int i=0; i<(int)vocabulary.size(); i++)
{
for (int j=0; j<(int)observations[0].size(); j++)
{
emission_p.at<double>(observations[0][j],obs[0]) = confidences[0][j];
}
V.at<double>(0,i) = start_p[i] * emission_p.at<double>(i,obs[0]);
path[i] = vocabulary.at(i);
}
// Run Viterbi for t > 0
for (int t=1; t<(int)obs.size(); t++)
{
//Dude this has to be done each time!!
emission_p = Mat::eye(62,62,CV_64FC1);
for (int e=0; e<(int)observations[t].size(); e++)
{
emission_p.at<double>(observations[t][e],obs[t]) = confidences[t][e];
}
vector<string> newpath(vocabulary.size());
for (int i=0; i<(int)vocabulary.size(); i++)
{
double max_prob = 0;
int best_idx = 0;
for (int j=0; j<(int)vocabulary.size(); j++)
{
double prob = V.at<double>(t-1,j) * transition_p.at<double>(j,i) * emission_p.at<double>(i,obs[t]);
if ( prob > max_prob)
{
max_prob = prob;
best_idx = j;
}
}
V.at<double>(t,i) = max_prob;
newpath[i] = path[best_idx] + vocabulary.at(i);
}
// Don't need to remember the old paths
path.swap(newpath);
}
double max_prob = 0;
int best_idx = 0;
for (int i=0; i<(int)vocabulary.size(); i++)
{
double prob = V.at<double>((int)obs.size()-1,i);
if ( prob > max_prob)
{
max_prob = prob;
best_idx = i;
}
}
//cout << path[best_idx] << endl;
out_sequence = out_sequence+" "+path[best_idx];
if (component_rects != NULL)
component_rects->push_back(words_rect[w]);
if (component_texts != NULL)
component_texts->push_back(path[best_idx]);
if (component_confidences != NULL)
component_confidences->push_back((float)max_prob);
}
return;
}
bool SequencePlayer::setTargetPose(const char* gname, const double *xyz, const double *rpy, double tm, const char* frame_name)
{
if ( m_debugLevel > 0 ) {
std::cerr << __PRETTY_FUNCTION__ << std::endl;
}
Guard guard(m_mutex);
if (!setInitialState()) return false;
// setup
std::vector<int> indices;
hrp::dvector start_av, end_av;
std::vector<hrp::dvector> avs;
if (! m_seq->getJointGroup(gname, indices) ) {
std::cerr << "[setTargetPose] Could not find joint group " << gname << std::endl;
return false;
}
start_av.resize(indices.size());
end_av.resize(indices.size());
//std::cerr << std::endl;
if ( ! m_robot->joint(indices[0])->parent ) {
std::cerr << "[setTargetPose] " << m_robot->joint(indices[0])->name << " does not have parent" << std::endl;
return false;
}
string base_parent_name = m_robot->joint(indices[0])->parent->name;
string target_name = m_robot->joint(indices[indices.size()-1])->name;
// prepare joint path
hrp::JointPathExPtr manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(base_parent_name), m_robot->link(target_name), dt, true, std::string(m_profile.instance_name)));
// calc fk
for (unsigned int i=0; i<m_robot->numJoints(); i++){
hrp::Link *j = m_robot->joint(i);
if (j) j->q = m_qRef.data.get_buffer()[i];
}
m_robot->calcForwardKinematics();
for ( unsigned int i = 0; i < manip->numJoints(); i++ ){
start_av[i] = manip->joint(i)->q;
}
// xyz and rpy are relateive to root link, where as pos and rotatoin of manip->calcInverseKinematics are relative to base link
// ik params
hrp::Vector3 start_p(m_robot->link(target_name)->p);
hrp::Matrix33 start_R(m_robot->link(target_name)->R);
hrp::Vector3 end_p(xyz[0], xyz[1], xyz[2]);
hrp::Matrix33 end_R = m_robot->link(target_name)->calcRfromAttitude(hrp::rotFromRpy(rpy[0], rpy[1], rpy[2]));
// change start and end must be relative to the frame_name
if ( (frame_name != NULL) && (! m_robot->link(frame_name) ) ) {
std::cerr << "[setTargetPose] Could not find frame_name " << frame_name << std::endl;
return false;
} else if ( frame_name != NULL ) {
hrp::Vector3 frame_p(m_robot->link(frame_name)->p);
hrp::Matrix33 frame_R(m_robot->link(frame_name)->attitude());
// fix start/end references from root to frame;
end_p = frame_R * end_p + frame_p;
end_R = frame_R * end_R;
}
manip->setMaxIKError(m_error_pos,m_error_rot);
manip->setMaxIKIteration(m_iteration);
std::cerr << "[setTargetPose] Solveing IK with frame" << frame_name << ", Error " << m_error_pos << m_error_rot << ", Iteration " << m_iteration << std::endl;
std::cerr << " Start " << start_p << start_R<< std::endl;
std::cerr << " End " << end_p << end_R<< std::endl;
// interpolate & calc ik
int len = max(((start_p - end_p).norm() / 0.02 ), // 2cm
((hrp::omegaFromRot(start_R.transpose() * end_R).norm()) / 0.025)); // 2 deg
len = max(len, 1);
std::vector<const double *> v_pos;
std::vector<double> v_tm;
v_pos.resize(len);
v_tm.resize(len);
// do loop
for (int i = 0; i < len; i++ ) {
double a = (1+i)/(double)len;
hrp::Vector3 p = (1-a)*start_p + a*end_p;
hrp::Vector3 omega = hrp::omegaFromRot(start_R.transpose() * end_R);
hrp::Matrix33 R = start_R * rodrigues(omega.isZero()?omega:omega.normalized(), a*omega.norm());
bool ret = manip->calcInverseKinematics2(p, R);
if ( m_debugLevel > 0 ) {
// for debug
std::cerr << "target pos/rot : " << i << "/" << a << " : "
<< p[0] << " " << p[1] << " " << p[2] << ","
<< omega[0] << " " << omega[1] << " " << omega[2] << std::endl;
}
if ( ! ret ) {
std::cerr << "[setTargetPose] IK failed" << std::endl;
return false;
}
v_pos[i] = (const double *)malloc(sizeof(double)*manip->numJoints());
for ( unsigned int j = 0; j < manip->numJoints(); j++ ){
((double *)v_pos[i])[j] = manip->joint(j)->q;
}
v_tm[i] = tm/len;
}
if ( m_debugLevel > 0 ) {
// for debug
for(int i = 0; i < len; i++ ) {
std::cerr << v_tm[i] << ":";
for(int j = 0; j < start_av.size(); j++ ) {
std::cerr << v_pos[i][j] << " ";
}
std::cerr << std::endl;
}
}
bool ret = m_seq->playPatternOfGroup(gname, v_pos, v_tm, m_qInit.data.get_buffer(), v_pos.size()>0?indices.size():0);
// clean up memory, need to improve
for (int i = 0; i < len; i++ ) {
free((double *)v_pos[i]);
}
return ret;
}
