void Object3D::set_rotation(float pitch, float yaw, float roll)
{
glm::vec3 pos_tmp(position()), scale_tmp(scale());
m_transform = glm::mat4_cast(glm::quat(glm::vec3(pitch, yaw, roll)));
set_position(pos_tmp);
set_scale(scale_tmp);
}
void Object3D::set_rotation(const glm::mat3 &theRot)
{
glm::vec3 pos_tmp(position()), scale_tmp(scale());
m_transform = glm::mat4(theRot);
set_position(pos_tmp);
set_scale(scale_tmp);
}
void initialise_prior_pdf(int init_pmf_pos,int init_pmf_type,pf::Point_mass_filter& pmf, double x, double y, double z){
// int test_type = 0;
arma::colvec3 pos_peg = {{x,y,z}};
arma::colvec3 pos_socket, pos_tmp;
std::cout<< "init_pmf_type: " << init_pmf_type << std::endl;
{
tf::StampedTransform transform;
opti_rviz::Listener::get_tf_once("world","link_socket",transform);
opti_rviz::type_conv::tf2vec(transform.getOrigin(),pos_socket);
}
pf::Point_mass_filter::delta delta_ ;
pf::Point_mass_filter::length length_;
init_delta_length(init_pmf_type,delta_,length_);
pos_tmp = pos_socket;
pos_tmp(0) = pos_tmp(0) + 0.01;
//pos_tmp.print("pos_tmp");
pmf.reset(pos_tmp,delta_,length_);
}