void ViewerWidget::appendToCameraPath(int id, const octomath::Pose6D& pose) { qglviewer::Vec position(pose.trans().x(), pose.trans().y(), pose.trans().z()); qglviewer::Quaternion quaternion = poseToQGLQuaternion(pose); qglviewer::Frame frame(position, quaternion); if(!camera()->keyFrameInterpolator(id)) { camera()->setKeyFrameInterpolator(id, new qglviewer::KeyFrameInterpolator(camera()->frame())); } camera()->keyFrameInterpolator(id)->addKeyFrame(frame); }
void Pointcloud::transform(octomath::Pose6D transform) { for (unsigned int i=0; i<points.size(); i++) { points[i] = transform.transform(points[i]); } // FIXME: not correct for multiple transforms current_inv_transform = transform.inv(); }
qglviewer::Quaternion ViewerWidget::poseToQGLQuaternion(const octomath::Pose6D& pose) { // copying octomap::Quaternion parameters to qglviewer::Quaternion does not work (reason unknown) // octomath::Quaternion quaternion = pose.rot().normalized(); // return qglviewer::Quaternion(quaternion.x(), quaternion.y(), quaternion.z(), quaternion.u()); // Compute viewing direction and use code from libqglviewer's "look at" function octomath::Vector3 dir = pose.rot().rotate(octomath::Vector3(1.,0.,0.)); qglviewer::Vec direction(dir.x(), dir.y(), dir.z()); //qglviewer::Vec xAxis = direction ^ camera()->upVector(); // useing 0, 0, 1 as upvector instead: qglviewer::Vec xAxis = direction ^ qglviewer::Vec(0.0, 0.0, 1.0); qglviewer::Quaternion q; q.setFromRotatedBasis(xAxis, xAxis^direction, -direction); return q; }
void ViewerWidget::setCamPose(const octomath::Pose6D& pose){ octomath::Pose6D ahead = pose * octomath::Pose6D(octomath::Vector3(1,0,0), octomath::Quaternion()); setCamPosition(pose.x(), pose.y(), pose.z(), ahead.x(), ahead.y(), ahead.z()); }
bool ReachabilityDummyInterface::isReachable(const octomath::Pose6D &pose) const { Capability cap; if (pose.y() > 0.04 && pose.y() < 0.06 && pose.z() > 0.44 && pose.z() < 0.46) { if (pose.x() > 0.14 && pose.x() < 0.16) { cap = Capability(SPHERE, 0.0, 0.0, 0.0); } else if (pose.x() > 0.34 && pose.x() < 0.36) { cap = Capability(CONE, 0.0, 90.0, 15.0); } else if (pose.x() > 0.54 && pose.x() < 0.56) { cap = Capability(CONE, 90.0, 0.0, 45.0); } else if (pose.x() > 0.74 && pose.x() < 0.76) { cap = Capability(CONE, 90.0, 0.0, 125.0); } else if (pose.x() > 0.94 && pose.x() < 0.96) { cap = Capability(CONE, 180.0, 120.0, 70.0); } else if (pose.x() > 1.14 && pose.x() < 1.16) { cap = Capability(CYLINDER_1, 0.0, 0.0, 10.0); } else if (pose.x() > 1.34 && pose.x() < 1.36) { cap = Capability(CYLINDER_1, 0.0, 0.0, 90.0); } else if (pose.x() > 1.54 && pose.x() < 1.56) { cap = Capability(CYLINDER_1, 40.0, 50.0, 45.0); } else if (pose.x() > 1.74 && pose.x() < 1.76) { cap = Capability(CYLINDER_2, 0.0, 0.0, 10.0); } else if (pose.x() > 1.94 && pose.x() < 1.96) { cap = Capability(CYLINDER_2, 0.0, 0.0, 45.0); } else if (pose.x() > 2.14 && pose.x() < 2.16) { cap = Capability(CYLINDER_2, 90.0, 90.0, 89.0); } else { return false; } } else { return false; } octomath::Vector3 unitVector(1.0, 0.0, 0.0); octomath::Vector3 rotatedVector = pose.rot().rotate(unitVector); double phi = atan2(rotatedVector.y(), rotatedVector.x()) * 180.0 / M_PI; double theta = acos(rotatedVector.z()) * 180.0 / M_PI; return cap.isDirectionPossible(phi, theta); }