本文整理汇总了C++中eigen::Affine3d::rotate方法的典型用法代码示例。如果您正苦于以下问题：C++ Affine3d::rotate方法的具体用法？C++ Affine3d::rotate怎么用？C++ Affine3d::rotate使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::Affine3d的用法示例。
在下文中一共展示了Affine3d::rotate方法的5个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: prepareFinalPointcloud
void RotatingRaySensor::prepareFinalPointcloud(){
// Copies current full pointcloud to pointcloud_full.
poseMutex.lock();
Eigen::Affine3d current_pose2 = current_pose;
Eigen::Affine3d rot;
rot.setIdentity();
rot.rotate(config.transf_sensor_rot_to_sensor);
poseMutex.unlock();
std::list<utils::Vector>::iterator it = fromCloud->begin();
pointcloud_full.clear();
pointcloud_full.reserve(fromCloud->size());
base::Vector3d vec_local;
for(int i=0; it != fromCloud->end(); it++, i++) {
// Transforms the pointcloud back from world to current node (see receiveDate()).
// In addition 'transf_sensor_rot_to_sensor' is applied which describes
// the orientation of the sensor in the unturned sensor frame.
vec_local = rot * current_pose2.inverse() * (*it);
pointcloud_full.push_back(vec_local);
}
//LOG_DEBUG("[RotatingRaySensor::prepareFinalPointcloud]: Pointcloud size: %d", pointcloud_full.size());
fromCloud->clear();
}示例2: transformEigenToTF
TEST(TFEigenConversions, eigen_tf_transform)
{
tf::Transform t;
Eigen::Affine3d k;
Eigen::Quaterniond kq;
kq.coeffs()(0) = gen_rand(-1.0,1.0);
kq.coeffs()(1) = gen_rand(-1.0,1.0);
kq.coeffs()(2) = gen_rand(-1.0,1.0);
kq.coeffs()(3) = gen_rand(-1.0,1.0);
kq.normalize();
k.translate(Eigen::Vector3d(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
k.rotate(kq);
transformEigenToTF(k,t);
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);
}
}
}示例3: sac_icp
icp_result sac_icp(PointCloudT::Ptr object, PointCloudT::Ptr scene, Eigen::Affine3d seed_pose) {
/* Apply ICP at many different offsets and orientations to seek a more accurate pose estimate */
// This is good, allow configurable distance-offset
Eigen::Vector3d offsets[] = {
Eigen::Vector3d(0., 0., 0. ),
Eigen::Vector3d(-0.06, 0, 0),
Eigen::Vector3d(0, -0.06, 0),
Eigen::Vector3d(0, 0, -0.06),
Eigen::Vector3d(0.06, 0, 0 ),
Eigen::Vector3d(0, 0.06, 0 ),
Eigen::Vector3d(0, 0, 0.06 )
};
// -> Do this better (try 45 deg increments, try auto-generating)
orientation orientations[] = {
{Eigen::Vector3d::UnitZ(), 0.0},
{Eigen::Vector3d::UnitZ(), M_PI / 5},
{Eigen::Vector3d::UnitZ(), 2 * M_PI / 5},
{Eigen::Vector3d::UnitZ(), 3 * M_PI / 5},
{Eigen::Vector3d::UnitZ(), 4 * M_PI / 5},
{Eigen::Vector3d::UnitZ(), M_PI},
{Eigen::Vector3d::UnitX(), M_PI / 5},
{Eigen::Vector3d::UnitX(), 2 * M_PI / 5},
{Eigen::Vector3d::UnitX(), 3 * M_PI / 5},
{Eigen::Vector3d::UnitX(), 4 * M_PI / 5},
{Eigen::Vector3d::UnitX(), M_PI},
{Eigen::Vector3d::UnitY(), M_PI / 5},
{Eigen::Vector3d::UnitY(), 2 * M_PI / 5},
{Eigen::Vector3d::UnitY(), 3 * M_PI / 5},
{Eigen::Vector3d::UnitY(), 4 * M_PI / 5},
{Eigen::Vector3d::UnitY(), M_PI},
};
// void affine_cloud(Eigen::Vector3f axis, float theta, Eigen::Vector3f translation, pcl::PointCloud<pcl::PointXYZ>& input_cloud,
// pcl::PointCloud<pcl::PointXYZ>& destination_cloud) {
pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_scene(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*scene, *xyz_scene); // Try this with pointNormals (Can we do this?)
pcl::visualization::PCLVisualizer viewer ("Point Cloud Visualization ENHANCED!");
viewer.setSize (1280, 1024); // Visualiser window size
viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> scene_color_h(xyz_scene, 255, 0, 0);
viewer.addPointCloud(xyz_scene, scene_color_h, "scene");
pcl::PointCloud<pcl::PointXYZ>::Ptr affined_cloud(new pcl::PointCloud<pcl::PointXYZ>);
viewer.addPointCloud(affined_cloud, "object");
// Should be using .reset() instead of new, right? I duno
// Eigen::Affine3d initial_transform;
// Eigen::Affine3d transform;
Eigen::Matrix3d seed_rotation = seed_pose.linear();
Eigen::Vector3d seed_translation = seed_pose.translation();
for (int k = 0; k < 7; k++) {
for (int j = 0; j < 16; j++) {
pcl::console::print_highlight ("At %i %i\n", k, j);
affined_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*object, *affined_cloud);
Eigen::Matrix3d rotation = seed_rotation * (Eigen::AngleAxisd(orientations[j].theta, orientations[j].axis));
Eigen::Vector3d translation = seed_translation + offsets[k];
Eigen::Affine3d transform = Eigen::Affine3d::Identity();
transform.rotate(rotation);
transform.translation() = translation;
pcl::transformPointCloud(*affined_cloud, *affined_cloud, transform);
// affine_cloud(orientations[j].axis, orientations[j].theta, offsets[k], *affined_cloud, *affined_cloud);
/*icp_result result = */apply_icp(affined_cloud, xyz_scene, 20, 0.01);
// /*icp_result result = */apply_icp(affined_cloud, xyz_scene, 40, 0.01);
// icp_result result = apply_icp(affined_cloud, xyz_scene, 40, 0.005);
// affine_cloud(result.affine, *affined_cloud, *affined_cloud);
viewer.updatePointCloud(affined_cloud, "object");
viewer.updatePointCloud(xyz_scene, scene_color_h, "scene");
next_iteration = false;
// visualize(affined_cloud, xyz_scene, "Post-icp");
while(next_iteration == false) {
viewer.spinOnce();
};
}
}
}示例4: main
// Usage: ./Volumetricd.exe ../../data/monkey.obj 256 4 2 90
int main(int argc, char **argv)
{
if (argc < 6)
{
std::cerr << "Missing parameters. Abort."
<< std::endl
<< "Usage: ./Volumetricd.exe ../../data/monkey.obj 256 8 2 90"
<< std::endl;
return EXIT_FAILURE;
}
Timer timer;
const std::string filepath = argv[1];
const int vol_size = atoi(argv[2]);
const int vx_size = atoi(argv[3]);
const int cloud_count = atoi(argv[4]);
const int rot_interval = atoi(argv[5]);
std::pair<std::vector<double>, std::vector<double>> depth_buffer;
//
// Projection and Modelview Matrices
//
Eigen::Matrix4d K = perspective_matrix(fov_y, aspect_ratio, near_plane, far_plane);
std::pair<Eigen::Matrix4d, Eigen::Matrix4d> T(Eigen::Matrix4d::Identity(), Eigen::Matrix4d::Identity());
//
// Creating volume
//
Eigen::Vector3d voxel_size(vx_size, vx_size, vx_size);
Eigen::Vector3d volume_size(vol_size, vol_size, vol_size);
Eigen::Vector3d voxel_count(volume_size.x() / voxel_size.x(), volume_size.y() / voxel_size.y(), volume_size.z() / voxel_size.z());
//
Eigen::Affine3d grid_affine = Eigen::Affine3d::Identity();
grid_affine.translate(Eigen::Vector3d(0, 0, -256));
grid_affine.scale(Eigen::Vector3d(1, 1, -1)); // z is negative inside of screen
Grid grid(volume_size, voxel_size, grid_affine.matrix());
//
// Importing .obj
//
timer.start();
std::vector<Eigen::Vector3d> points3DOrig, pointsTmp;
import_obj(filepath, points3DOrig);
timer.print_interval("Importing monkey : ");
std::cout << "Monkey point count : " << points3DOrig.size() << std::endl;
//
// Translating and rotating monkey point cloud
std::pair<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>> cloud;
//
Eigen::Affine3d rotate = Eigen::Affine3d::Identity();
Eigen::Affine3d translate = Eigen::Affine3d::Identity();
translate.translate(Eigen::Vector3d(0, 0, -256));
//
// Compute first cloud
//
for (Eigen::Vector3d p3d : points3DOrig)
{
Eigen::Vector4d rot = translate.matrix() * rotate.matrix() * p3d.homogeneous();
rot /= rot.w();
cloud.first.push_back(rot.head<3>());
}
//
// Update grid with first cloud
//
timer.start();
create_depth_buffer<double>(depth_buffer.first, cloud.first, K, Eigen::Matrix4d::Identity(), far_plane);
timer.print_interval("CPU compute depth : ");
timer.start();
update_volume(grid, depth_buffer.first, K, T.first);
timer.print_interval("CPU Update volume : ");
//
// Compute next clouds
Eigen::Matrix4d cloud_mat = Eigen::Matrix4d::Identity();
Timer iter_timer;
for (int i = 1; i < cloud_count; ++i)
{
std::cout << std::endl << i << " : " << i * rot_interval << std::endl;
iter_timer.start();
// Rotation matrix
rotate = Eigen::Affine3d::Identity();
rotate.rotate(Eigen::AngleAxisd(DegToRad(i * rot_interval), Eigen::Vector3d::UnitY()));
cloud.second.clear();
for (Eigen::Vector3d p3d : points3DOrig)
{
Eigen::Vector4d rot = translate.matrix() * rotate.matrix() * p3d.homogeneous();
rot /= rot.w();
cloud.second.push_back(rot.head<3>());
}
//.........这里部分代码省略.........
示例5: rot
void smurf::Robot::loadFromSmurf(const std::string& path)
{
configmaps::ConfigMap map;
// parse joints from model
boost::filesystem::path filepath(path);
model = smurf_parser::parseFile(&map, filepath.parent_path().generic_string(), filepath.filename().generic_string(), true);
//first we need to create all Frames
for (configmaps::ConfigVector::iterator it = map["frames"].begin(); it != map["frames"].end(); ++it)
{
configmaps::ConfigMap &fr(it->children);
Frame *frame = new Frame(fr["name"]);
availableFrames.push_back(frame);
//std::cout << "Adding additional frame " << frame->getName() << std::endl;
}
for(std::pair<std::string, boost::shared_ptr<urdf::Link>> link: model->links_)
{
Frame *frame = new Frame(link.first);
for(boost::shared_ptr<urdf::Visual> visual : link.second->visual_array)
{
frame->addVisual(*visual);
}
availableFrames.push_back(frame);
//std::cout << "Adding link frame " << frame->getName() << std::endl;
}
for(std::pair<std::string, boost::shared_ptr<urdf::Joint> > jointIt: model->joints_)
{
boost::shared_ptr<urdf::Joint> joint = jointIt.second;
//std::cout << "Adding joint " << joint->name << std::endl;
Frame *source = getFrameByName(joint->parent_link_name);
Frame *target = getFrameByName(joint->child_link_name);
//TODO this might not be set in some cases, perhaps force a check
configmaps::ConfigMap annotations;
for(configmaps::ConfigItem &cv : map["joints"])
{
if(static_cast<std::string>(cv.children["name"]) == joint->name)
{
annotations = cv.children;
}
}
switch(joint->type)
{
case urdf::Joint::FIXED:
{
const urdf::Pose &tr(joint->parent_to_joint_origin_transform);
StaticTransformation *transform = new StaticTransformation(source, target,
Eigen::Quaterniond(tr.rotation.w, tr.rotation.x, tr.rotation.y, tr.rotation.z),
Eigen::Vector3d(tr.position.x, tr.position.y, tr.position.z));
staticTransforms.push_back(transform);
}
break;
case urdf::Joint::FLOATING:
{
DynamicTransformation *transform = new DynamicTransformation(source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"));
dynamicTransforms.push_back(transform);
Eigen::Vector3d axis(joint->axis.x, joint->axis.y, joint->axis.z);
Eigen::Affine3d sourceToAxis(Eigen::Affine3d::Identity());
sourceToAxis.translation() = axis;
base::JointLimitRange limits;
const urdf::Pose parentToOrigin(joint->parent_to_joint_origin_transform);
Eigen::Quaterniond rot(parentToOrigin.rotation.w, parentToOrigin.rotation.x, parentToOrigin.rotation.y, parentToOrigin.rotation.z);
Eigen::Vector3d trans(parentToOrigin.position.x, parentToOrigin.position.y, parentToOrigin.position.z);
Eigen::Affine3d parentToOriginAff;
parentToOriginAff.setIdentity();
parentToOriginAff.rotate(rot);
parentToOriginAff.translation() = trans;
Joint *smurfJoint = new Joint (source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"), checkGet(annotations, "driver"), limits, sourceToAxis, parentToOriginAff, joint);
joints.push_back(smurfJoint);
}
break;
case urdf::Joint::REVOLUTE:
case urdf::Joint::CONTINUOUS:
case urdf::Joint::PRISMATIC:
{
base::JointState minState;
minState.position = joint->limits->lower;
minState.effort = 0;
minState.speed = 0;
base::JointState maxState;
maxState.position = joint->limits->upper;
maxState.effort = joint->limits->effort;
maxState.speed = joint->limits->velocity;
base::JointLimitRange limits;
limits.min = minState;
limits.max = maxState;
Eigen::Vector3d axis(joint->axis.x, joint->axis.y, joint->axis.z);
Eigen::Affine3d sourceToAxis(Eigen::Affine3d::Identity());
sourceToAxis.translation() = axis;
//.........这里部分代码省略.........