gsl_vector* VelStereoMatcher::stereoToVec(const StereoProperties stereo)
{
Eigen::Affine3f tform = stereo.getLeftCamera().tform;
Eigen::Matrix4f intrinsics = stereo.getLeftCamera().intrinsics.matrix();
Eigen::Vector3f tran;
tran = tform.translation();
float x = tran[0];
float y = tran[1];
float z = tran[2];
Eigen::Matrix3f mat = tform.rotation();
Eigen::AngleAxisf axis;
axis = mat;
float ax = axis.axis()[0] * axis.angle();
float ay = axis.axis()[1] * axis.angle();
float az = axis.axis()[2] * axis.angle();
float fx = intrinsics(0,0);
float fy = intrinsics(1,1);
float cx = intrinsics(0,2);
float cy = intrinsics(1,2);
float baseline = stereo.baseline;
gsl_vector* vec = gsl_vector_alloc(11);
gsl_vector_set(vec, 0, x);
gsl_vector_set(vec, 1, y);
gsl_vector_set(vec, 2, z);
gsl_vector_set(vec, 3, ax);
gsl_vector_set(vec, 4, ay);
gsl_vector_set(vec, 5, az);
gsl_vector_set(vec, 6, fx);
gsl_vector_set(vec, 7, fy);
gsl_vector_set(vec, 8, cx);
gsl_vector_set(vec, 9, cy);
gsl_vector_set(vec, 10, baseline);
return vec;
}
vtkSmartPointer<vtkDataSet>
pcl::visualization::createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation,
double width, double height, double depth)
{
// coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth]
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
t->Identity ();
t->Translate (translation.x (), translation.y (), translation.z ());
Eigen::AngleAxisf a (rotation);
t->RotateWXYZ (pcl::rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
cube->SetXLength (width);
cube->SetYLength (height);
cube->SetZLength (depth);
vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
tf->SetTransform (t);
tf->SetInputConnection (cube->GetOutputPort ());
return (tf->GetOutput ());
}
vtkSmartPointer<vtkDataSet>
pcl::visualization::createCube (const pcl::ModelCoefficients &coefficients)
{
// coefficients = [Tx, Ty, Tz, Qx, Qy, Qz, Qw, width, height, depth]
vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
t->Identity ();
t->Translate (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
Eigen::AngleAxisf a (Eigen::Quaternionf (coefficients.values[6], coefficients.values[3],
coefficients.values[4], coefficients.values[5]));
t->RotateWXYZ (pcl::rad2deg (a.angle ()), a.axis ()[0], a.axis ()[1], a.axis ()[2]);
vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
cube->SetXLength (coefficients.values[7]);
cube->SetYLength (coefficients.values[8]);
cube->SetZLength (coefficients.values[9]);
vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
tf->SetTransform (t);
tf->SetInputConnection (cube->GetOutputPort ());
return (tf->GetOutput ());
}
void X3DConverter::startShape(const std::array<float, 12>& matrix) {
// Finding axis/angle from matrix using Eigen for its bullet proof implementation.
Eigen::Transform<float, 3, Eigen::Affine> t;
t.setIdentity();
for (unsigned int i = 0; i < 3; i++) {
for (unsigned int j = 0; j < 4; j++) {
t(i, j) = matrix[i+j*3];
}
}
Eigen::Matrix3f rotationMatrix;
Eigen::Matrix3f scaleMatrix;
t.computeRotationScaling(&rotationMatrix, &scaleMatrix);
Eigen::Quaternionf q;
Eigen::AngleAxisf aa;
q = rotationMatrix;
aa = q;
Eigen::Vector3f scale = scaleMatrix.diagonal();
Eigen::Vector3f translation = t.translation();
startNode(ID::Transform);
m_writers.back()->setSFVec3f(ID::translation, translation.x(), translation.y() , translation.z());
m_writers.back()->setSFRotation(ID::rotation, aa.axis().x(), aa.axis().y(), aa.axis().z(), aa.angle());
m_writers.back()->setSFVec3f(ID::scale, scale.x(), scale.y(), scale.z());
startNode(ID::Shape);
startNode(ID::Appearance);
startNode(ID::Material);
m_writers.back()->setSFColor<vector<float> >(ID::diffuseColor, RVMColorHelper::color(m_materials.back()));
endNode(ID::Material); // Material
endNode(ID::Appearance); // Appearance
}
void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
std::ostringstream s;
s << "Feedback from marker '" << feedback->marker_name << "' "
<< " / control '" << feedback->control_name << "'";
switch ( feedback->event_type )
{
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
mouseInUse = true;
break;
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
{
// Compute FK for end effectors that have changed
// Call IK to get the joint states
moveit_msgs::GetPositionFKRequest req;
req.fk_link_names.push_back("RightArm");
req.robot_state.joint_state = planState;
req.header.stamp = ros::Time::now();
moveit_msgs::GetPositionFKResponse resp;
gFKinClient.call(req, resp);
std::cerr << "Response: " << resp.pose_stamped[0] << std::endl;
if (resp.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
{
joyInt.currentPose = resp.pose_stamped[0];
}
else
{
ROS_ERROR_STREAM("Failed to solve FK: " << resp.error_code.val);
}
gRPosePublisher.publish(joyInt.currentPose);
mouseInUse = false;
break;
}
case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
{
Eigen::AngleAxisf aa;
aa = Eigen::Quaternionf(feedback->pose.orientation.w,
feedback->pose.orientation.x,
feedback->pose.orientation.y,
feedback->pose.orientation.z);
boost::shared_ptr<const urdf::Joint> targetJoint = huboModel.getJoint(feedback->marker_name);
Eigen::Vector3f axisVector = hubo_motion_ros::toEVector3(targetJoint->axis);
float angle;
// Get sign of angle
if (aa.axis().dot(axisVector) < 0)
{
angle = -aa.angle();
}
else
{
angle = aa.angle();
}
// Trim angle to joint limits
if (angle > targetJoint->limits->upper)
{
angle = targetJoint->limits->upper;
}
else if (angle < targetJoint->limits->lower)
{
angle = targetJoint->limits->lower;
}
// Locate the index of the solution joint in the plan state
for (int j = 0; j < planState.name.size(); j++)
{
if (feedback->marker_name == planState.name[j])
{
planState.position[j] = angle;
}
}
prevAA = aa;
// Time and Frame stamps
planState.header.frame_id = "/Body_TSY";
planState.header.stamp = ros::Time::now();
gStatePublisher.publish(planState);
break;
}
}
gIntServer->applyChanges();
}