本文整理汇总了C++中eigen::Affine3d类的典型用法代码示例。如果您正苦于以下问题:C++ Affine3d类的具体用法?C++ Affine3d怎么用?C++ Affine3d使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Affine3d类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: TestFind3DAffineTransform
void TestFind3DAffineTransform(){
// Create datasets with known transform
Eigen::Matrix3Xd in(3, 100), out(3, 100);
Eigen::Quaternion<double> Q(1, 3, 5, 2);
Q.normalize();
Eigen::Matrix3d R = Q.toRotationMatrix();
double scale = 2.0;
for (int row = 0; row < in.rows(); row++) {
for (int col = 0; col < in.cols(); col++) {
in(row, col) = log(2*row + 10.0)/sqrt(1.0*col + 4.0) + sqrt(col*1.0)/(row + 1.0);
}
}
Eigen::Vector3d S;
S << -5, 6, -27;
for (int col = 0; col < in.cols(); col++)
out.col(col) = scale*R*in.col(col) + S;
Eigen::Affine3d A = Find3DAffineTransform(in, out);
// See if we got the transform we expected
if ( (scale*R-A.linear()).cwiseAbs().maxCoeff() > 1e-13 ||
(S-A.translation()).cwiseAbs().maxCoeff() > 1e-13)
throw "Could not determine the affine transform accurately enough";
}
开发者ID:lood339,项目名称:opencv_util,代码行数:25,代码来源:Kabsch.cpp
示例2: checkTolerance
bool CartesianTrajectoryAction::checkTolerance(Eigen::Affine3d err, cartesian_trajectory_msgs::CartesianTolerance tol) {
if ((tol.position.x > 0.0) && (fabs(err.translation().x()) > tol.position.x)) {
return false;
}
if ((tol.position.y > 0.0) && (fabs(err.translation().y()) > tol.position.y)) {
return false;
}
if ((tol.position.z > 0.0) && (fabs(err.translation().z()) > tol.position.z)) {
return false;
}
Eigen::AngleAxisd ax(err.rotation());
Eigen::Vector3d rot = ax.axis() * ax.angle();
if ((tol.rotation.x > 0.0) && (fabs(rot(0)) > tol.rotation.x)) {
return false;
}
if ((tol.rotation.y > 0.0) && (fabs(rot(1)) > tol.rotation.y)) {
return false;
}
if ((tol.rotation.z > 0.0) && (fabs(rot(2)) > tol.rotation.z)) {
return false;
}
return true;
}
开发者ID:mikolak,项目名称:common_controllers,代码行数:30,代码来源:cartesian_trajectory_action.cpp
示例3: srvSetHuboObjectPose
bool srvSetHuboObjectPose(HuboApplication::SetHuboObjectPose::Request &req,
HuboApplication::SetHuboObjectPose::Response &res)
{
bool response = true;
Eigen::Affine3d tempPose;
Eigen::Isometry3d armPose;
tf::poseMsgToEigen(req.Target, tempPose);
if (tempPose(0,3) != tempPose(0,3)) // null check
{
res.Success = false;
return false;
}
armPose = tempPose.matrix();
//std::cerr << tempPose.matrix() << std::endl;
m_Manip.setControlMode(OBJECT_POSE);
m_Manip.setAngleMode(QUATERNION);
m_Manip.setPose(armPose, req.ObjectIndex);
m_Manip.sendCommand();
//std::cerr << armPose.matrix() << std::endl;
res.Success = response;
return response;
}
开发者ID:a-price,项目名称:HuboApplication,代码行数:26,代码来源:ros_hubo_interface.cpp
示例4: srvSetHuboArmPose
bool srvSetHuboArmPose(HuboApplication::SetHuboArmPose::Request &req,
HuboApplication::SetHuboArmPose::Response &res)
{
bool response = true;
Eigen::Affine3d tempPose;
Eigen::Isometry3d armPose;
tf::poseMsgToEigen(req.Target, tempPose);
if (tempPose(0,3) != tempPose(0,3)) // null check
{
res.Success = false;
return false;
}
//std::cerr << tempPose.matrix() << std::endl;
armPose = tempPose.matrix();
//armPose.translation() = tempPose.translation();
//armPose.linear() = tempPose.rotation();
//std::cerr << armPose.matrix() << std::endl;
m_Manip.setControlMode(END_EFFECTOR);
m_Manip.setAngleMode(QUATERNION);
m_Manip.setPose(armPose, req.ArmIndex);
m_Manip.sendCommand();
//std::cerr << armPose.matrix() << std::endl;
res.Success = response;
return response;
}
开发者ID:a-price,项目名称:HuboApplication,代码行数:29,代码来源:ros_hubo_interface.cpp
示例5: updateCollisionObjectPose
void MotionPlanningFrame::updateCollisionObjectPose(bool update_marker_position)
{
QList<QListWidgetItem *> sel = ui_->collision_objects_list->selectedItems();
if (sel.empty())
return;
planning_scene_monitor::LockedPlanningSceneRW ps = planning_display_->getPlanningSceneRW();
if (ps)
{
collision_detection::CollisionWorld::ObjectConstPtr obj = ps->getWorld()->getObject(sel[0]->text().toStdString());
if (obj && obj->shapes_.size() == 1)
{
Eigen::Affine3d p;
p.translation()[0] = ui_->object_x->value();
p.translation()[1] = ui_->object_y->value();
p.translation()[2] = ui_->object_z->value();
p = Eigen::Translation3d(p.translation()) *
(Eigen::AngleAxisd(ui_->object_rx->value(), Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(ui_->object_ry->value(), Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ui_->object_rz->value(), Eigen::Vector3d::UnitZ()));
ps->getWorldNonConst()->moveShapeInObject(obj->id_, obj->shapes_[0], p);
planning_display_->queueRenderSceneGeometry();
// Update the interactive marker pose to match the manually introduced one
if (update_marker_position && scene_marker_)
{
Eigen::Quaterniond eq(p.rotation());
scene_marker_->setPose(Ogre::Vector3(ui_->object_x->value(), ui_->object_y->value(), ui_->object_z->value()),
Ogre::Quaternion(eq.w(), eq.x(), eq.y(), eq.z()), "");
}
}
}
}
开发者ID:digideskio,项目名称:vigir_manipulation_planning,代码行数:34,代码来源:motion_planning_frame_objects.cpp
示例6: copyPointCloud
void
RGBID_SLAM::VisualizationManager::getPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr whole_point_cloud)
{
IdToPoseMap::iterator id2pose_map_it;
IdToPointCloudMap::const_iterator id2pointcloud_map_it;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr aux_point_cloud;
aux_point_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
for (id2pose_map_it = id2pose_map_.begin();
id2pose_map_it != id2pose_map_.end();
id2pose_map_it++)
{
int kf_id = (*id2pose_map_it).first;
Eigen::Affine3d pose = (*id2pose_map_it).second;
id2pointcloud_map_it = id2pointcloud_map_.find(kf_id);
if (id2pointcloud_map_it != id2pointcloud_map_.end())
{
aux_point_cloud->clear();
copyPointCloud((*id2pointcloud_map_it).second, aux_point_cloud);
(*id2pointcloud_map_it).second->clear();
alignPointCloud(aux_point_cloud, pose.linear(), pose.translation());
*whole_point_cloud += *aux_point_cloud;
}
}
}
开发者ID:dorian3d,项目名称:RGBiD-SLAM,代码行数:30,代码来源:visualization_manager.cpp
示例7: transformTFToEigen
TEST(TFEigenConversions, tf_eigen_transform)
{
tf::Transform t;
tf::Quaternion tq;
tq[0] = gen_rand(-1.0,1.0);
tq[1] = gen_rand(-1.0,1.0);
tq[2] = gen_rand(-1.0,1.0);
tq[3] = gen_rand(-1.0,1.0);
tq.normalize();
t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
t.setRotation(tq);
Eigen::Affine3d k;
transformTFToEigen(t,k);
for(int i=0; i < 3; i++)
{
ASSERT_NEAR(t.getOrigin()[i],k.matrix()(i,3),1e-6);
for(int j=0; j < 3; j++)
{
ASSERT_NEAR(t.getBasis()[i][j],k.matrix()(i,j),1e-6);
}
}
for (int col = 0 ; col < 3; col ++)
ASSERT_NEAR(k.matrix()(3, col), 0, 1e-6);
ASSERT_NEAR(k.matrix()(3,3), 1, 1e-6);
}
开发者ID:AhmedAnsariIIT,项目名称:iitmabhiyanros,代码行数:28,代码来源:test_eigen_tf.cpp
示例8: toIsometry
Eigen::Isometry3d toIsometry(const Eigen::Affine3d& pose)
{
Eigen::Isometry3d p(pose.rotation());
p.translation() = pose.translation();
return p;
}
开发者ID:liubingkarin,项目名称:dvo_slam,代码行数:7,代码来源:local_map.cpp
示例9: fabs
bool kinematic_constraints::VisibilityConstraint::equal(const KinematicConstraint &other, double margin) const
{
if (other.getType() != type_)
return false;
const VisibilityConstraint &o = static_cast<const VisibilityConstraint&>(other);
if (target_frame_id_ == o.target_frame_id_ && sensor_frame_id_ == o.sensor_frame_id_ &&
cone_sides_ == o.cone_sides_ && sensor_view_direction_ == o.sensor_view_direction_)
{
if (fabs(max_view_angle_ - o.max_view_angle_) > margin ||
fabs(target_radius_ - o.target_radius_) > margin)
return false;
Eigen::Affine3d diff = sensor_pose_.inverse() * o.sensor_pose_;
if (diff.translation().norm() > margin)
return false;
if (!diff.rotation().isIdentity(margin))
return false;
diff = target_pose_.inverse() * o.target_pose_;
if (diff.translation().norm() > margin)
return false;
if (!diff.rotation().isIdentity(margin))
return false;
return true;
}
return false;
}
开发者ID:jonbinney,项目名称:moveit-core,代码行数:26,代码来源:kinematic_constraint.cpp
示例10: if
void Irp6pInverseKinematic::updateHook() {
if (port_input_wrist_pose_.read(wrist_pose_) == RTT::NewData) {
Eigen::Affine3d trans;
tf::poseMsgToEigen(wrist_pose_, trans);
port_current_joint_position_.read(local_current_joints_);
inverse_kinematics_single_iteration(local_current_joints_, trans,
&local_desired_joints_);
port_output_joint_position_.write(local_desired_joints_);
} else if (port_input_end_effector_pose_.read(end_effector_pose_)
== RTT::NewData) {
port_tool_.read(tool_msgs_);
Eigen::Affine3d tool;
Eigen::Affine3d trans;
Eigen::Affine3d wrist_pose;
tf::poseMsgToEigen(end_effector_pose_, trans);
tf::poseMsgToEigen(tool_msgs_, tool);
wrist_pose = trans * tool.inverse();
port_current_joint_position_.read(local_current_joints_);
inverse_kinematics_single_iteration(local_current_joints_, wrist_pose,
&local_desired_joints_);
port_output_joint_position_.write(local_desired_joints_);
}
}
开发者ID:oleszczy,项目名称:irp6_robot,代码行数:35,代码来源:Irp6pInverseKinematic.cpp
示例11:
bool kinematic_constraints::PositionConstraint::equal(const KinematicConstraint &other, double margin) const
{
if (other.getType() != type_)
return false;
const PositionConstraint &o = static_cast<const PositionConstraint&>(other);
if (link_model_ == o.link_model_ && constraint_frame_id_ == o.constraint_frame_id_)
{
if ((offset_ - o.offset_).norm() > margin)
return false;
if (constraint_region_.size() != o.constraint_region_.size())
return false;
for (std::size_t i = 0 ; i < constraint_region_.size() ; ++i)
{
Eigen::Affine3d diff = constraint_region_pose_[i].inverse() * o.constraint_region_pose_[i];
if (diff.translation().norm() > margin)
return false;
if (!diff.rotation().isIdentity(margin))
return false;
if (fabs(constraint_region_[i]->computeVolume() - o.constraint_region_[i]->computeVolume()) >= margin)
return false;
}
return true;
}
return false;
}
开发者ID:jonbinney,项目名称:moveit-core,代码行数:26,代码来源:kinematic_constraint.cpp
示例12: updateTransformations
bool SensorProcessorBase::updateTransformations(const std::string& sensorFrameId,
const ros::Time& timeStamp)
{
try {
transformListener_.waitForTransform(sensorFrameId, mapFrameId_, timeStamp, ros::Duration(1.0));
tf::StampedTransform transformTf;
transformListener_.lookupTransform(mapFrameId_, sensorFrameId, timeStamp, transformTf);
poseTFToEigen(transformTf, transformationSensorToMap_);
transformListener_.lookupTransform(robotBaseFrameId_, sensorFrameId, timeStamp, transformTf); // TODO Why wrong direction?
Eigen::Affine3d transform;
poseTFToEigen(transformTf, transform);
rotationBaseToSensor_.setMatrix(transform.rotation().matrix());
translationBaseToSensorInBaseFrame_.toImplementation() = transform.translation();
transformListener_.lookupTransform(mapFrameId_, robotBaseFrameId_, timeStamp, transformTf); // TODO Why wrong direction?
poseTFToEigen(transformTf, transform);
rotationMapToBase_.setMatrix(transform.rotation().matrix());
translationMapToBaseInMapFrame_.toImplementation() = transform.translation();
return true;
} catch (tf::TransformException &ex) {
ROS_ERROR("%s", ex.what());
return false;
}
}
开发者ID:amiltonwong,项目名称:elevation_mapping,代码行数:27,代码来源:SensorProcessorBase.cpp
示例13: q
/*!
* \brief affine3d2UrdfPose converts an Eigen affine 4x4 matrix o represent the pose into a urdf pose
* vparam pose eigen Affine3d pose
* \return urdf pose with position and rotation.
*/
RCS::Pose Affine3d2UrdfPose(const Eigen::Affine3d &pose) {
RCS::Pose p;
p.getOrigin().setX(pose.translation().x());
p.getOrigin().setY(pose.translation().y());
p.getOrigin().setZ(pose.translation().z());
Eigen::Quaterniond q (pose.rotation());
tf::Quaternion qtf(q.x(),q.y(),q.z(),q.w());
//std::cout << "Affine3d2UrdfPose Quaterion = \n" << q.x() << ":" << q.y() << ":" << q.z() << ":" << q.w() << std::endl;
p.setRotation(qtf);
//std::cout << "After Affine3d2UrdfPose Quaterion = \n" << p.getRotation().x() << ":" << p.getRotation().y() << ":" << p.getRotation().z() << ":" << p.getRotation().w() << std::endl;
#if 0
MatrixEXd m = pose.rotation();
Eigen::Quaterniond q = EMatrix2Quaterion(m);
Eigen::Quaterniond q(pose.rotation());
p.getRotation().setX(q.x());
p.getRotation().setY(q.y());
p.getRotation().setZ(q.z());
p.getRotation().setW(q.w());
#endif
return p;
}
开发者ID:usnistgov,项目名称:el-robotics-core,代码行数:30,代码来源:Conversions.cpp
示例14: pose_moveTo
void UpperBodyPlanner::pose_moveTo(const std::string &link_name, const Eigen::Affine3d& desired_pose, const int &step_num, std::vector<geometry_msgs::Pose> &pose_sequence) {
Eigen::Affine3d current_pose = kinematic_state->getGlobalLinkTransform(link_name);
Eigen::Quaterniond current_q = Rmat2Quaternion(current_pose.rotation());
Eigen::Quaterniond desired_q = Rmat2Quaternion(desired_pose.rotation());
Eigen::Quaterniond step_q;
std::cout << "**********************************" << std::endl;
std::cout << "end_effector_name is: " << link_name << std::endl;
std::cout << "Current translation is: " << current_pose.translation().transpose() << std::endl;
std::cout << "current rotation is: " << std::endl;
std::cout << current_pose.rotation() << std::endl;
std::cout << "current quaternion is: "<< std::endl;
std::cout << "w = " << current_q.w() << ", x = " << current_q.x() << ", y = " << current_q.y() << ", z = " << current_q.z() << std::endl;
std::cout << "Desired translation is: " << desired_pose.translation().transpose() << std::endl;
std::cout << "Desired rotation is: " << std::endl;
std::cout << desired_pose.rotation() << std::endl;
std::cout << "Desired quaternion is: " << std::endl;
std::cout << "w = " << desired_q.w() << ", x = " << desired_q.x() << ", y = " << desired_q.y() << ", z = " << desired_q.z() << std::endl;
std::cout << "**********************************" << std::endl;
Eigen::Vector3d step_translation_movement = (desired_pose.translation() - current_pose.translation()) / step_num;
for (int i = 0; i < step_num; i++) {
Eigen::Affine3d step_target;
step_target.translation() = current_pose.translation() + (i + 1) * step_translation_movement;
step_q = current_q.slerp((i + 1.0) / step_num, desired_q);
geometry_msgs::Pose target_pose = Eigen2msgPose(step_target.translation(), step_q);
pose_sequence.push_back(target_pose);
}
}
开发者ID:nianxing,项目名称:Telemanipulation_Atlas_Using_Razer_Hydra,代码行数:29,代码来源:hydra_move_group.cpp
示例15: toTransform
/**
* @brief transform matrix which represents the pose transform as a 4x4
* homogenous matrix
*
* @result 4x4 homogenous transform matrix
*/
Eigen::Affine3d toTransform() const
{
Eigen::Affine3d t;
t = orientation;
t.pretranslate( position );
return t;
}
开发者ID:arneboe,项目名称:base-types,代码行数:13,代码来源:Pose.hpp
示例16: kstate
TEST_F(DistanceFieldCollisionDetectionTester, LinksInCollision)
{
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res1;
collision_detection::CollisionResult res2;
collision_detection::CollisionResult res3;
//req.contacts = true;
//req.max_contacts = 100;
req.group_name = "whole_body";
planning_models::KinematicState kstate(kmodel_);
kstate.setToDefaultValues();
Eigen::Affine3d offset = Eigen::Affine3d::Identity();
offset.translation().x() = .01;
kstate.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
kstate.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
acm_->setEntry("base_link", "base_bellow_link", false);
crobot_->checkSelfCollision(req, res1, kstate, *acm_);
ASSERT_TRUE(res1.collision);
acm_->setEntry("base_link", "base_bellow_link", true);
crobot_->checkSelfCollision(req, res2, kstate, *acm_);
ASSERT_FALSE(res2.collision);
// req.verbose = true;
kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);
acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
crobot_->checkSelfCollision(req, res3, kstate, *acm_);
ASSERT_TRUE(res3.collision);
}
开发者ID:jonbinney,项目名称:moveit-core,代码行数:34,代码来源:test_collision_distance_field.cpp
示例17: commandForce
void commandForce(const geometry_msgs::WrenchStamped::ConstPtr &msg)
{
F_des_ << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z,
msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z;
if(msg->header.frame_id == root_name_)
return;
geometry_msgs::PoseStamped in_root;
in_root.pose.orientation.w = 1.0;
in_root.header.frame_id = msg->header.frame_id;
try {
tf_.waitForTransform(root_name_, msg->header.frame_id, msg->header.stamp, ros::Duration(0.1));
tf_.transformPose(root_name_, in_root, in_root);
}
catch (const tf::TransformException &ex)
{
ROS_ERROR("Failed to transform: %s", ex.what());
return;
}
Eigen::Affine3d t;
tf::poseMsgToEigen(in_root.pose, t);
F_des_.head<3>() = t.linear() * F_des_.head<3>();
F_des_.tail<3>() = t.linear() * F_des_.tail<3>();
}
开发者ID:gt-ros-pkg,项目名称:hrl-pr2-behaviors,代码行数:28,代码来源:hybrid_force_controller.cpp
示例18: myCallback1
void myCallback1(const interaction_cursor_msgs::InteractionCursorUpdate poseSubscribed) {
// std::cout<<" i am in mycallback "<<std::endl;
target1 = Eigen::Affine3d::Identity();
Eigen::Quaterniond q(poseSubscribed.pose.pose.orientation.w,
poseSubscribed.pose.pose.orientation.x,
poseSubscribed.pose.pose.orientation.y,
poseSubscribed.pose.pose.orientation.z);
translation1 << poseSubscribed.pose.pose.position.x,
poseSubscribed.pose.pose.position.y,
poseSubscribed.pose.pose.position.z;
std::cout << " 3 " << std::endl;
rotation1 = q.toRotationMatrix();
target1.translation() = translation1;
target1.linear() = rotation1;
std::cout << "please move hydra to a proper place and click the button. " << std::endl;
if (poseSubscribed.button_state == 1) {
button1 = true;
}
}
开发者ID:nianxing,项目名称:Telemanipulation_Atlas_Using_Razer_Hydra,代码行数:30,代码来源:main.cpp
示例19: isValidScene
/* checks if a given scene is in static equilibrium or not */
bool SceneValidator::isValidScene(std::vector<string> modelnames, std::vector<Eigen::Affine3d> model_poses){
//set all the Objects's positions
num = modelnames.size();
for (int i =0; i < num; i++){
auto mappedObject= m.find(modelnames[i]); //get model from hashmap
Eigen::Affine3d a = model_poses[i];
const dMatrix3 R = { //convert affine info to a 3x3 rotation matrix and a x,y,z position array
a(0,0), a(0,1), a(0,2),
a(1,0), a(1,1), a(1,2),
a(2,0), a(2,1), a(2,2) };
const dReal center[3] = {a.translation()[0],a.translation()[1], a.translation()[2]};
translateObject(mappedObject->second, center, R); //get the model name's MyObject info and feed it the position and rotation
}
//complete series of checks to see if scene is still stable or not
if (!isStableStill(modelnames, STEP1)){ //check #1
return false;
} else
if (!isStableStill(modelnames, STEP2)){ //check #2
return false;
} else
if (!isStableStill(modelnames, STEP3)){ //check #3
return false;
} else
if (!isStableStill(modelnames, STEP4)){ //check #4
return false;
}
else{
return true;
}
}
开发者ID:jshepley14,项目名称:scenevalidator,代码行数:32,代码来源:sceneValidator.cpp
示例20: PoseAffineToGeomMsg
geometry_msgs::Pose PoseAffineToGeomMsg(const Eigen::Affine3d &e) {
geometry_msgs::Pose m;
m.position.x = e.translation().x();
m.position.y = e.translation().y();
m.position.z = e.translation().z();
// This is a column major vs row major matrice faux pas!
#if 0
MatrixEXd em = e.rotation();
Eigen::Quaterniond q = EMatrix2Quaterion(em);
#endif
Eigen::Quaterniond q(e.rotation());
m.orientation.x = q.x();
m.orientation.y = q.y();
m.orientation.z = q.z();
m.orientation.w = q.w();
#if 0
if (m.orientation.w < 0) {
m.orientation.x *= -1;
m.orientation.y *= -1;
m.orientation.z *= -1;
m.orientation.w *= -1;
}
#endif
}
开发者ID:usnistgov,项目名称:el-robotics-core,代码行数:25,代码来源:Conversions.cpp
注:本文中的eigen::Affine3d类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论