TranslationRotation3D &TranslationRotation3D::
operator*=(const TranslationRotation3D &rhs) {
// Fl = Fl*Fr;
Eigen::Map<Eigen::Vector3d> tra_left(T_);
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_left(R_mat_);
double T_rhs[3];
rhs.getT(T_rhs);
Eigen::Map<Eigen::Vector3d> tra_right(T_rhs);
double R_mat_rhs[9];
rhs.getR_mat(R_mat_rhs);
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_right(
R_mat_rhs);
tra_left = rot_left * tra_right + tra_left;
rot_left *= rot_right;
updateR();
// convert back to rotation matrix to increase numerical stability
updateR_mat();
// apply logical AND to validity
setValid(isValid() && rhs.isValid());
return *this;
}
void TranslationRotation3D::rotateZ(double angle_deg) {
double angle_rad = angle_deg * PI_ / 180.0;
double c = cos(angle_rad);
double s = sin(angle_rad);
Eigen::Matrix3d Rx;
Rx << c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0;
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot(R_mat_);
rot *= Rx;
updateR();
updateR_mat(); // for stability
}
/* Change user's orientation using keyboard */
GLvoid keyboard_callback(unsigned char key, int x, int y)
{
switch (key)
{
case 'x':
case 'y':
case 'z':
updateR(key,5.0, &orientation_angle, orientation_axis);
break;
case 'X':
case 'Y':
case 'Z':
updateR(key,-5.0, &orientation_angle, orientation_axis);
break;
case 27:
exit(0);
break;
}
glutPostRedisplay();
}
void UISystem::update( Float dt )
{
std::for_each(m_new_widgets.begin(), m_new_widgets.end(), [&](Widget *w){
WM_Widget mes;
mes.type = WMT_iuiCreate;
mes.from = w;
sendMessage(mes);
});
m_new_widgets.clear();
updateR(m_root, dt);
}
/* Special keyboard stroke callback */
GLvoid special_keyboard(GLint key, GLint x, GLint y)
{
switch (key) {
case GLUT_KEY_UP:
case GLUT_KEY_LEFT:
updateR(key, -5.0, &location_angle, location_axis);
break;
case GLUT_KEY_DOWN:
case GLUT_KEY_RIGHT:
updateR(key, 5.0, &location_angle, location_axis);
break;
case GLUT_KEY_PAGE_UP:
zoom_factor = zoom_factor * (1.0/1.1);
reshape(width, height);
break;
case GLUT_KEY_PAGE_DOWN:
zoom_factor = zoom_factor * 1.1;
reshape(width, height);
break;
default:
break;
}
glutPostRedisplay();
}
void UISystem::updateR( Widget *widget, Float dt )
{
// 子が先
widget->eachChildren([&](Widget *c){ updateR(c, dt); });
widget->update(dt);
}
void TranslationRotation3D::setR_mat(double *R_mat_in) {
for (int i = 0; i < 9; i++)
R_mat_[i] = R_mat_in[i];
updateR();
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
// Input checks
if (nrhs < 3) {
printUsg();
return;
}
#define S1 prhs[0]
#define S2 prhs[1]
if(!IS_REAL_2D(S1)) // check S1
mexErrMsgTxt("S1 must be a real 2D matrix.");
if(!IS_REAL_2D(S2)) // check S2
mexErrMsgTxt("S2 must be a real 2D matrix.");
// init
float* S1r = (float*)mxGetPr(S1);
float* S2r = (float*)mxGetPr(S2);
short B = static_cast<short>(mxGetM(S1)); // num of bins
short m = static_cast<short>(mxGetN(S1))-1; // length of Seq1
short n = static_cast<short>(mxGetN(S2))-1; // length of Seq2
// Get parameters
unsigned int maxIter;
short nRes;
short L;
string dist;
bool isVerbose;
getParams(nrhs, prhs, L, maxIter, nRes, dist, isVerbose);
Rset R1, R2; // Rectangle sets for splitting
float lb, ub; // lower and upper bounds
State topS; // top state
priority_queue<State, vector<State>, CompareState> Q; // priority queue
priority_queue<State, vector<State>, CompareState> Qtmp; // priority queue
if (dist.compare("l1")!=0 && dist.compare("l2")!=0 && dist.compare("X2")!=0 && dist.compare("int")!=0) {
mexPrintf("!!! We do not support \"%s\" distance. Please read the usage carefully!\n",dist.c_str());
mexPrintf("!!! Please select from these distance options: [\"l1\",\"l2\",\"X2\",\"int\"]\n");
return;
}
// Init output structure
int dimOut1[2] = {1,nRes};
int numFields1 = 3;
const char* fieldNames1[] = { "lo", "hi", "lb" };
plhs[0] = mxCreateStructArray(2, dimOut1, numFields1, fieldNames1); // creates fields
int dimOut2[2] = {1,nRes};
int numFields2 = 1;
const char* fieldNames2[] = { "nIter" };
plhs[1] = mxCreateStructArray(2, dimOut2, numFields2, fieldNames2); // creates fields
// Main loop below
mexPrintf("===============================================\n");
mexPrintf("+ Running TCD w/ %s distance\n", dist.c_str());
mexPrintf("===============================================\n");
for (short iRes=0; iRes<nRes; iRes++) {
// init
unsigned int nIter = 0; // counter for #iteration
bool isConv = false; // flag for convergence
Rset Rout = { {0,0,0,0}, {0,0,0,0}, false };
// Update Q
Rset R;
if (iRes == 0) {
Rset R0 = { {1,1,1,1}, {m,m,n,n}, true }; // init R with everything
R = R0;
} else {
Rout = topS.R;
mexPrintf("+ Updating Q (size=%d -> ", Q.size());
// Clean Qtmp
while (!Qtmp.empty())
Qtmp.pop();
// Copy Q into Qtmp and clean Q
while(!Q.empty()) {
Qtmp.push(Q.top());
Q.pop();
}
// Being to update Q
State tmpS;
while(!Qtmp.empty()) {
tmpS = Qtmp.top();
Qtmp.pop();
// Update R into R1 and R2
Rset R1, R2;
updateR(topS.R, tmpS.R, m, n, R1, R2);
// Push {bound(R1), R1} to Q
getBounds(S1r, S2r, B, R1, lb, ub, dist);
if (R1.isValid()) {
State St1 = {lb, R1};
Q.push(St1);
}
// Push {bound(R2), R2} to Q
getBounds(S1r, S2r, B, R2, lb, ub, dist);
if (R2.isValid()) {
State St2 = {lb, R2};
Q.push(St2);
}
}
mexPrintf("%d)\n",Q.size());
// Get topS
topS = Q.top();
R = topS.R;
Q.pop();
}
while (!isConv) {
nIter++;
// Split rect set into R1 and R2
split(R, R1, R2);
// Push R1 into queue if valid
if (isvalid(R1, L)) {
getBounds(S1r, S2r, B, R1, lb, ub, dist);
State S = {lb, R1};
Q.push(S);
}
// Push R2 into queue if valid
if (isvalid(R2, L)) {
getBounds(S1r, S2r, B, R2, lb, ub, dist);
State S = {lb, R2};
Q.push(S);
}
// Update R by top state
topS = Q.top();
R = topS.R;
Q.pop();
if (isOverlap(Rout, R)) {
mexPrintf("Rout: "); Rout.disp();
mexPrintf("R: "); R.disp();
return;
}
// Check stopping criterion
if (nIter % 100 == 0 && isVerbose) {
mexPrintf(" # Iter%4d: ",nIter);
topS.disp();
}
if (nIter >= maxIter) {
mexPrintf(" # MaxIter%d reached: ", maxIter);
topS.disp();
isConv = true;
}
if (isUniqR(R)) {
mexPrintf(" # Iter%4d: ",nIter);
topS.disp();
isConv = true;
}
} // end of main loop
// Assign output structure
setField<short>(&plhs[0], iRes, "lo", 4, mxINT16_CLASS, topS.R.lo); // set lo
setField<short>(&plhs[0], iRes, "hi", 4, mxINT16_CLASS, topS.R.hi); // set hi
setField<float>(&plhs[0], iRes, "lb", 1, mxSINGLE_CLASS, &topS.bound); // set lb
setField<unsigned int>(&plhs[1], iRes, "nIter", 1, mxUINT16_CLASS, &nIter); // set info.nIter
} // end of iRes
return;
}
