void MMCollisionInt::fit(int degree, doublereal deltastar,
doublereal* a, doublereal* b, doublereal* c)
{
int i, n = m_nmax - m_nmin + 1;
int ndeg=0;
vector_fp values(n);
doublereal rmserr;
vector_fp w(n);
doublereal* logT = DATA_PTR(m_logTemp) + m_nmin;
for (i = 0; i < n; i++) {
if (deltastar == 0.0) {
values[i] = astar_table[8*(i + m_nmin + 1)];
} else {
values[i] = poly5(deltastar, DATA_PTR(m_apoly[i+m_nmin]));
}
}
w[0]= -1.0;
rmserr = polyfit(n, logT, DATA_PTR(values),
DATA_PTR(w), degree, ndeg, 0.0, a);
for (i = 0; i < n; i++) {
if (deltastar == 0.0) {
values[i] = bstar_table[8*(i + m_nmin + 1)];
} else {
values[i] = poly5(deltastar, DATA_PTR(m_bpoly[i+m_nmin]));
}
}
w[0]= -1.0;
rmserr = polyfit(n, logT, DATA_PTR(values),
DATA_PTR(w), degree, ndeg, 0.0, b);
for (i = 0; i < n; i++) {
if (deltastar == 0.0) {
values[i] = cstar_table[8*(i + m_nmin + 1)];
} else {
values[i] = poly5(deltastar, DATA_PTR(m_cpoly[i+m_nmin]));
}
}
w[0]= -1.0;
rmserr = polyfit(n, logT, DATA_PTR(values),
DATA_PTR(w), degree, ndeg, 0.0, c);
if (DEBUG_MODE_ENABLED && m_loglevel > 2) {
writelogf("\nT* fit at delta* = %.6g\n", deltastar);
writelog("astar = [" + vec2str(vector_fp(a, a+degree+1))+ "]\n");
if (rmserr > 0.01) {
writelogf("Warning: RMS error = %12.6g for A* fit\n", rmserr);
}
writelog("bstar = [" + vec2str(vector_fp(b, b+degree+1))+ "]\n");
if (rmserr > 0.01) {
writelogf("Warning: RMS error = %12.6g for B* fit\n", rmserr);
}
writelog("cstar = [" + vec2str(vector_fp(c, c+degree+1))+ "]\n");
if (rmserr > 0.01) {
writelogf("Warning: RMS error = %12.6g for C* fit\n", rmserr);
}
}
}
int ArrayTest::validate_test_results( int test_case_idx )
{
static const char* arr_names[] = { "input", "input/output", "output",
"ref input/output", "ref output",
"temporary", "mask" };
size_t i, j;
prepare_to_validation( test_case_idx );
for( i = 0; i < 2; i++ )
{
int i0 = i == 0 ? OUTPUT : INPUT_OUTPUT;
int i1 = i == 0 ? REF_OUTPUT : REF_INPUT_OUTPUT;
size_t sizei = test_array[i0].size();
assert( sizei == test_array[i1].size() );
for( j = 0; j < sizei; j++ )
{
double err_level;
int code;
if( !test_array[i1][j] )
continue;
err_level = get_success_error_level( test_case_idx, i0, (int)j );
code = cmpEps2(ts, test_mat[i0][j], test_mat[i1][j], err_level, element_wise_relative_error, arr_names[i0]);
if (code == 0) continue;
for( i0 = 0; i0 < (int)test_array.size(); i0++ )
{
size_t sizei0 = test_array[i0].size();
if( i0 == REF_INPUT_OUTPUT || i0 == OUTPUT || i0 == TEMP )
continue;
for( i1 = 0; i1 < (int)sizei0; i1++ )
{
const Mat& arr = test_mat[i0][i1];
if( !arr.empty() )
{
string sizestr = vec2str(", ", &arr.size[0], arr.dims);
ts->printf( TS::LOG, "%s array %d type=%sC%d, size=(%s)\n",
arr_names[i0], i1, getTypeName(arr.depth()),
arr.channels(), sizestr.c_str() );
}
}
}
ts->set_failed_test_info( code );
return code;
}
}
return 0;
}
void dfs(TreeNode *root, vector<string> &ans, vector<int> &path) {
path.push_back(root->val);
if (root->left == nullptr && root->right == nullptr) {
ans.push_back(vec2str(path));
}
if (root->left != nullptr) {
dfs(root->left, ans, path);
}
if (root->right != nullptr) {
dfs(root->right, ans, path);
}
path.pop_back();
}
bool getJointNames(const std::string joint_list_param, const std::string urdf_param,
std::vector<std::string> & joint_names)
{
joint_names.clear();
// 1) Try to read explicit list of joint names
if (ros::param::has(joint_list_param) && getListParam(joint_list_param, joint_names))
{
ROS_INFO_STREAM("Found user-specified joint names in '" << joint_list_param << "': " << vec2str(joint_names));
return true;
}
else
ROS_WARN_STREAM("Unable to find user-specified joint names in '" << joint_list_param << "'");
// 2) Try to find joint names from URDF model
urdf::Model model;
if ( ros::param::has(urdf_param)
&& model.initParam(urdf_param)
&& findChainJointNames(model.getRoot(), true, joint_names) )
{
ROS_INFO_STREAM("Using joint names from URDF: '" << urdf_param << "': " << vec2str(joint_names));
return true;
}
else
ROS_WARN_STREAM("Unable to find URDF joint names in '" << urdf_param << "'");
// 3) Use default joint-names
const int NUM_JOINTS = 6; //Most robots have 6 joints
for (int i=0; i<NUM_JOINTS; ++i)
{
std::stringstream tmp;
tmp << "joint_" << i+1;
joint_names.push_back(tmp.str());
}
ROS_INFO_STREAM("Using standard 6-DOF joint names: " << vec2str(joint_names));
return true;
}
std::string twoDvec2str(std::vector<std::vector<T>> vec) const
{
std::string result = "{ ";
for (T i = 0; i < vec.size(); ++i)
{
result += vec2str(vec[i]);
if (i + 1 != vec.size()) result += ", ";
}
result += " }";
return result;
}
void M2MFstAligner::Sequences2FSTNoInit( VectorFst<LogArc>* fst, vector<string>* seq1, vector<string>* seq2 ){
/*
Build an FST that represents all possible alignments between seq1 and seq2, given the
parameter values input by the user. Here we encode the input and output labels, in fact
creating a WFSA. This simplifies the training process, but means that we can only
easily compute a joint maximization. In practice joint maximization seems to give the
best results anyway, so it probably doesn't matter.
*/
int istate=0; int ostate=0;
for( unsigned int i=0; i<=seq1->size(); i++ ){
for( unsigned int j=0; j<=seq2->size(); j++ ){
fst->AddState();
istate = i*(seq2->size()+1)+j;
//Epsilon arcs for seq1
if( seq1_del==true )
for( unsigned int l=1; l<=seq2_max; l++ ){
if( j+l<=seq2->size() ){
vector<string> subseq2( seq2->begin()+j, seq2->begin()+j+l );
int is = isyms->Find(skip+s1s2_sep+vec2str(subseq2, seq2_sep));
ostate = i*(seq2->size()+1) + (j+l);
LogArc arc( is, is, alignment_model[is], ostate );
_compute_penalties( arc.ilabel, 1, l, true, false );
fst->AddArc( istate, arc );
}
}
//Epsilon arcs for seq2
if( seq2_del==true )
for( unsigned int k=1; k<=seq1_max; k++ ){
if( i+k<=seq1->size() ){
vector<string> subseq1( seq1->begin()+i, seq1->begin()+i+k );
int is = isyms->Find(vec2str(subseq1, seq1_sep)+s1s2_sep+skip);
ostate = (i+k)*(seq2->size()+1) + j;
LogArc arc( is, is, alignment_model[is], ostate );
_compute_penalties( arc.ilabel, k, 1, false, true );
fst->AddArc( istate, arc );
}
}
//All the other arcs
for( unsigned int k=1; k<=seq1_max; k++ ){
for( unsigned int l=1; l<=seq2_max; l++ ){
if( i+k<=seq1->size() && j+l<=seq2->size() ){
vector<string> subseq1( seq1->begin()+i, seq1->begin()+i+k );
string s1 = vec2str(subseq1, seq1_sep);
vector<string> subseq2( seq2->begin()+j, seq2->begin()+j+l );
string s2 = vec2str(subseq2, seq2_sep);
if( restrict==true && l>1 && k>1)
continue;
int is = isyms->Find(s1+s1s2_sep+s2);
ostate = (i+k)*(seq2->size()+1) + (j+l);
LogArc arc( is, is, alignment_model[is], ostate );
_compute_penalties( arc.ilabel, k, l, false, false );
fst->AddArc( istate, arc );
}
}
}
}
}
fst->SetStart(0);
fst->SetFinal( ((seq1->size()+1)*(seq2->size()+1))-1, LogWeight::One() );
//Unless seq1_del==true && seq2_del==true we will have unconnected states
// thus we need to run connect to clean out these states
if( seq1_del==false || seq2_del==false )
Connect(fst);
return;
}
void M2MFstAligner::Sequences2FST( VectorFst<LogArc>* fst, vector<string>* seq1, vector<string>* seq2 ){
/*
Build an FST that represents all possible alignments between seq1 and seq2, given the
parameter values input by the user. Here we encode the input and output labels, in fact
creating a WFSA. This simplifies the training process, but means that we can only
easily compute a joint maximization. In practice joint maximization seems to give the
best results anyway, so it probably doesn't matter.
Note: this also performs the initizization routine. It performs a UNIFORM initialization
meaning that every non-null alignment sequence is eventually initialized to 1/Num(unique_alignments).
It might be more appropriate to consider subsequence length here, but for now we stick
to the m2m-aligner approach.
TODO: Add an FST version and support for conditional maximization. May be useful for languages
like Japanese where there is a distinct imbalance in the seq1->seq2 length correspondences.
*/
int istate=0; int ostate=0;
for( unsigned int i=0; i<=seq1->size(); i++ ){
for( unsigned int j=0; j<=seq2->size(); j++ ){
fst->AddState();
istate = i*(seq2->size()+1)+j;
//Epsilon arcs for seq1
if( seq1_del==true )
for( unsigned int l=1; l<=seq2_max; l++ ){
if( j+l<=seq2->size() ){
vector<string> subseq2( seq2->begin()+j, seq2->begin()+j+l );
int is = isyms->AddSymbol(skip+s1s2_sep+vec2str(subseq2, seq2_sep));
ostate = i*(seq2->size()+1) + (j+l);
LogArc arc( is, is, 99, ostate );
fst->AddArc( istate, arc );
/*
if( prev_alignment_model.find(arc.ilabel)==prev_alignment_model.end() ){
prev_alignment_model.insert( pair<LogArc::Label,LogWeight>(arc.ilabel,arc.weight) );
_compute_penalties( arc.ilabel, 1, l, true, false );
}else{
prev_alignment_model[arc.ilabel] = Plus(prev_alignment_model[arc.ilabel],arc.weight);
}
total = Plus( total, arc.weight );
*/
}
}
//Epsilon arcs for seq2
if( seq2_del==true )
for( unsigned int k=1; k<=seq1_max; k++ ){
if( i+k<=seq1->size() ){
vector<string> subseq1( seq1->begin()+i, seq1->begin()+i+k );
int is = isyms->AddSymbol(vec2str(subseq1, seq1_sep)+s1s2_sep+skip);
ostate = (i+k)*(seq2->size()+1) + j;
LogArc arc( is, is, 99, ostate );
fst->AddArc( istate, arc );
/*
if( prev_alignment_model.find(arc.ilabel)==prev_alignment_model.end() ){
prev_alignment_model.insert( pair<LogArc::Label,LogWeight>(arc.ilabel,arc.weight) );
_compute_penalties( arc.ilabel, k, 1, false, true );
}else{
prev_alignment_model[arc.ilabel] = Plus(prev_alignment_model[arc.ilabel],arc.weight);
}
total = Plus(total, arc.weight);
*/
}
}
//All the other arcs
for( unsigned int k=1; k<=seq1_max; k++ ){
for( unsigned int l=1; l<=seq2_max; l++ ){
if( i+k<=seq1->size() && j+l<=seq2->size() ){
vector<string> subseq1( seq1->begin()+i, seq1->begin()+i+k );
string s1 = vec2str(subseq1, seq1_sep);
vector<string> subseq2( seq2->begin()+j, seq2->begin()+j+l );
string s2 = vec2str(subseq2, seq2_sep);
//This says only 1-M and N-1 allowed, no M-N links!
if( restrict==true && l>1 && k>1)
continue;
int is = isyms->AddSymbol(s1+s1s2_sep+s2);
ostate = (i+k)*(seq2->size()+1) + (j+l);
LogArc arc( is, is, LogWeight::One().Value()*(k+l), ostate );
fst->AddArc( istate, arc );
//During the initialization phase, just count non-eps transitions
//We currently initialize to uniform probability so there is also
// no need to tally anything here.
/*
if( prev_alignment_model.find(arc.ilabel)==prev_alignment_model.end() ){
prev_alignment_model.insert( pair<LogArc::Label,LogWeight>(arc.ilabel, arc.weight) );
_compute_penalties( arc.ilabel, k, l, false, false );
}else{
prev_alignment_model[arc.ilabel] = Plus(prev_alignment_model[arc.ilabel],arc.weight);
}
total = Plus( total, arc.weight );
*/
}
}
}
}
}
fst->SetStart(0);
fst->SetFinal( ((seq1->size()+1)*(seq2->size()+1))-1, LogWeight::One() );
//Unless seq1_del==true && seq2_del==true we will have unconnected states
// thus we need to run connect to clean out these states
if( seq1_del==false || seq2_del==false )
Connect(fst);
//Only add arcs that are in the FINAL fst to the model
for( StateIterator<VectorFst<LogArc> > siter(*fst); !siter.Done(); siter.Next() ){
LogArc::StateId q = siter.Value();
for( ArcIterator<VectorFst<LogArc> > aiter(*fst, q); !aiter.Done(); aiter.Next() ){
const LogArc& arc = aiter.Value();
if( prev_alignment_model.find(arc.ilabel)==prev_alignment_model.end() ){
prev_alignment_model.insert( pair<LogArc::Label,LogWeight>(arc.ilabel, arc.weight) );
string sym = isyms->Find(arc.ilabel);
size_t del = sym.find("}");
size_t ski = sym.find("_");
size_t chu = sym.find("|");
int k=1; int l=1;
bool xd = false; bool yd = false;
if( chu!=string::npos ){
if( chu<del )
k += 1;
else
l += 1;
}
if( ski!=string::npos ){
if( ski<del )
xd = true;
else
yd = true;
}
_compute_penalties( arc.ilabel, k, l, false, false );
}else{
prev_alignment_model[arc.ilabel] = Plus(prev_alignment_model[arc.ilabel],arc.weight);
}
total = Plus( total, arc.weight );
}
}
return;
}
void
M2MFstAligner::Sequences2FST(VectorFst<LogArc> *fst,
vector<string> *seq1,
vector<string> *seq2)
{
/*
Build an FST that represents all possible alignments between seq1 and seq2, given the
parameter values input by the user. Here we encode the input and output labels, in fact
creating a WFSA. This simplifies the training process, but means that we can only
easily compute a joint maximization. In practice joint maximization seems to give the
best results anyway, so it probably doesn't matter.
Note: this also performs the initizization routine. It performs a UNIFORM initialization
meaning that every non-null alignment sequence is eventually initialized to 1/Num(unique_alignments).
It might be more appropriate to consider subsequence length here, but for now we stick
to the m2m-aligner approach.
TODO: Add an FST version and support for conditional maximization. May be useful for languages
like Japanese where there is a distinct imbalance in the seq1->seq2 length correspondences.
*/
int istate = 0;
int ostate = 0;
for (int i = 0; i <= seq1->size(); i++) {
for (int j = 0; j <= seq2->size(); j++) {
fst->AddState();
istate = i * (seq2->size() + 1) + j;
//Epsilon arcs for seq1
if (seq1_del == true)
for (int l = 1; l <= seq2_max; l++) {
if (j + l <= seq2->size()) {
vector<string> subseq2(seq2->begin() + j,
seq2->begin() + j + l);
int is =
isyms->AddSymbol(skip + s1s2_sep +
vec2str(subseq2, seq2_sep));
ostate = i * (seq2->size() + 1) + (j + l);
//LogArc arc( is, is, LogWeight::One().Value()*(l+1)*2, ostate );
LogArc arc(is, is, 99, ostate);
//LogArc arc( is, is, LogWeight::Zero(), ostate );
fst->AddArc(istate, arc);
if (prev_alignment_model.find(arc.ilabel) ==
prev_alignment_model.end())
prev_alignment_model.insert(pair <
LogArc::Label,
LogWeight >
(arc.ilabel,
arc.weight));
else
prev_alignment_model[arc.ilabel] =
Plus(prev_alignment_model[arc.ilabel],
arc.weight);
total = Plus(total, arc.weight);
}
}
//Epsilon arcs for seq2
if (seq2_del == true)
for (int k = 1; k <= seq1_max; k++) {
if (i + k <= seq1->size()) {
vector<string> subseq1(seq1->begin() + i,
seq1->begin() + i + k);
int is =
isyms->AddSymbol(vec2str(subseq1, seq1_sep) +
s1s2_sep + skip);
ostate = (i + k) * (seq2->size() + 1) + j;
//LogArc arc( is, is, LogWeight::One().Value()*(k+1)*2, ostate );
LogArc arc(is, is, 99, ostate);
//LogArc arc( is, is, LogWeight::Zero(), ostate );
fst->AddArc(istate, arc);
if (prev_alignment_model.find(arc.ilabel) ==
prev_alignment_model.end())
prev_alignment_model.insert(pair <
LogArc::Label,
LogWeight >
(arc.ilabel,
arc.weight));
else
prev_alignment_model[arc.ilabel] =
Plus(prev_alignment_model[arc.ilabel],
arc.weight);
total = Plus(total, arc.weight);
}
}
//All the other arcs
for (int k = 1; k <= seq1_max; k++) {
for (int l = 1; l <= seq2_max; l++) {
if (i + k <= seq1->size() && j + l <= seq2->size()) {
vector<string> subseq1(seq1->begin() + i,
seq1->begin() + i + k);
string s1 = vec2str(subseq1, seq1_sep);
vector<string> subseq2(seq2->begin() + j,
seq2->begin() + j + l);
string s2 = vec2str(subseq2, seq2_sep);
if (l > 1 && k > 1)
continue;
int is = isyms->AddSymbol(s1 + s1s2_sep + s2);
ostate = (i + k) * (seq2->size() + 1) + (j + l);
LogArc arc(is, is,
LogWeight::One().Value() * (k + l),
ostate);
//LogArc arc( is, is, LogWeight::One().Value(), ostate );
fst->AddArc(istate, arc);
//During the initialization phase, just count non-eps transitions
//We currently initialize to uniform probability so there is also
// no need to tally anything here.
if (prev_alignment_model.find(arc.ilabel) ==
prev_alignment_model.end())
prev_alignment_model.insert(pair <
LogArc::Label,
LogWeight >
(arc.ilabel,
arc.weight));
else
prev_alignment_model[arc.ilabel] =
Plus(prev_alignment_model[arc.ilabel],
arc.weight);
total = Plus(total, arc.weight);
}
}
}
}
}
fst->SetStart(0);
fst->SetFinal(((seq1->size() + 1) * (seq2->size() + 1)) - 1,
LogWeight::One());
//Unless seq1_del==true && seq2_del==true we will have unconnected states
// thus we need to run connect to clean out these states
//fst->SetInputSymbols(isyms);
//fst->Write("right.nc.fsa");
if (seq1_del == false or seq2_del == false)
Connect(fst);
//fst->Write("right.c.fsa");
return;
}
void MMCollisionInt::init(doublereal tsmin, doublereal tsmax, int log_level)
{
m_loglevel = log_level;
if (DEBUG_MODE_ENABLED && m_loglevel > 0) {
writelog("Collision Integral Polynomial Fits\n");
}
m_nmin = -1;
m_nmax = -1;
for (int n = 0; n < 37; n++) {
if (tsmin > tstar[n+1]) {
m_nmin = n;
}
if (tsmax > tstar[n+1]) {
m_nmax = n+1;
}
}
if (m_nmin < 0 || m_nmin >= 36 || m_nmax < 0 || m_nmax > 36) {
m_nmin = 0;
m_nmax = 36;
}
if (DEBUG_MODE_ENABLED && m_loglevel > 0) {
writelogf("T*_min = %g\n", tstar[m_nmin + 1]);
writelogf("T*_max = %g\n", tstar[m_nmax + 1]);
}
m_logTemp.resize(37);
doublereal rmserr, e22 = 0.0, ea = 0.0, eb = 0.0, ec = 0.0;
if (DEBUG_MODE_ENABLED && m_loglevel > 0) {
writelog("Collision integral fits at each tabulated T* vs. delta*.\n"
"These polynomial fits are used to interpolate between "
"columns (delta*)\n in the Monchick and Mason tables."
" They are only used for nonzero delta*.\n");
if (log_level < 4) {
writelog("polynomial coefficients not printed (log_level < 4)\n");
}
}
for (int i = 0; i < 37; i++) {
m_logTemp[i] = log(tstar[i+1]);
vector_fp c(DeltaDegree+1);
rmserr = fitDelta(0, i, DeltaDegree, DATA_PTR(c));
if (DEBUG_MODE_ENABLED && log_level > 3) {
writelogf("\ndelta* fit at T* = %.6g\n", tstar[i+1]);
writelog("omega22 = [" + vec2str(c) + "]\n");
}
m_o22poly.push_back(c);
e22 = std::max(e22, rmserr);
rmserr = fitDelta(1, i, DeltaDegree, DATA_PTR(c));
m_apoly.push_back(c);
if (DEBUG_MODE_ENABLED && log_level > 3) {
writelog("A* = [" + vec2str(c) + "]\n");
}
ea = std::max(ea, rmserr);
rmserr = fitDelta(2, i, DeltaDegree, DATA_PTR(c));
m_bpoly.push_back(c);
if (DEBUG_MODE_ENABLED && log_level > 3) {
writelog("B* = [" + vec2str(c) + "]\n");
}
eb = std::max(eb, rmserr);
rmserr = fitDelta(3, i, DeltaDegree, DATA_PTR(c));
m_cpoly.push_back(c);
if (DEBUG_MODE_ENABLED && log_level > 3) {
writelog("C* = [" + vec2str(c) + "]\n");
}
ec = std::max(ec, rmserr);
if (DEBUG_MODE_ENABLED && log_level > 0) {
writelogf("max RMS errors in fits vs. delta*:\n"
" omega_22 = %12.6g \n"
" A* = %12.6g \n"
" B* = %12.6g \n"
" C* = %12.6g \n", e22, ea, eb, ec);
}
}
}
void dumpVec(const QVector2D &vec) {std::cerr << vec2str(vec) << std::endl;}
