/** \brief Resets the MultilevelPatch.
*
* @param idx - feature ID
* @initTime - Time at initialization.
*/
void reset(){
H_.setIdentity();
e0_ = 0;
e1_ = 0;
s_ = 0;
for(unsigned int i = 0;i<nLevels_;i++){
isValidPatch_[i] = false;
}
}
Eigen::Matrix3f GraspPlannerEvaluatorWindow::rodriguesFormula(Eigen::Vector3f vector, float theta) {
//Rotation vector must be of unit length
//Angle theta must be in rad
Eigen::Matrix3f mat;
mat.setIdentity();
Eigen::Matrix3f skewSym = getSkewSymmetricMatrix(vector);
mat = mat + skewSym*sin(theta) + skewSym*skewSym*(1 - cos(theta));
return mat;
}
