本文整理汇总了C++中eigen::Vector4d::w方法的典型用法代码示例。如果您正苦于以下问题:C++ Vector4d::w方法的具体用法?C++ Vector4d::w怎么用?C++ Vector4d::w使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::Vector4d
的用法示例。
在下文中一共展示了Vector4d::w方法的6个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1:
Visualization::Visualization(bot_lcmgl_t* lcmgl, const StereoCalibration* calib)
: _lcmgl(lcmgl),
_calibration(calib)
{
// take the Z corresponding to disparity 5 px as 'max Z'
Eigen::Matrix4d uvdtoxyz = calib->getUvdToXyz();
Eigen::Vector4d xyzw = uvdtoxyz * Eigen::Vector4d(1, 1, 5, 1);
xyzw /= xyzw.w();
_max_z = xyzw.z();
// take the Z corresponding to 3/4 disparity img width px as 'min Z'
xyzw = uvdtoxyz * Eigen::Vector4d(1, 1, (3*calib->getWidth())/4, 1);
xyzw /= xyzw.w();
_min_z = xyzw.z();
}
示例2:
Eigen::Vector3d
StereoDisparity::getXyzValues(int u, int v, float disparity)
{
Eigen::Vector4d uvd1((double) u, (double) v, disparity, 1);
Eigen::Vector4d xyzw = (*_uvd1_to_xyz) * uvd1;
return xyzw.head<3>() / xyzw.w();
}
示例3: fitPlaneError
// Calculate the fitting error of a plane to certain 3D data
double PlaneFit::fitPlaneError(const Points3D& P, const Eigen::Vector4d& coeff)
{
// Calculate the normalised equation coefficients (to put the resulting error in units of normal distance to plane)
Eigen::Vector3d normal = coeff.head<3>();
double norm = normal.norm();
double scale = (norm > 0.0 ? norm : coeff.w());
Eigen::Vector3d abc = normal / scale;
double d = coeff.w() / scale;
// Calculate the average distance to the plane of the data points
double err = 0.0;
size_t N = P.size();
for(size_t i = 0; i < N; i++)
err += fabs(abc.dot(P[i]) + d)/N;
// Return the calculated error
return err;
}
示例4: world
Eigen::Vector2i
pcl::visualization::worldToView (const Eigen::Vector4d &world_pt, const Eigen::Matrix4d &view_projection_matrix, int width, int height)
{
// Transform world to clipping coordinates
Eigen::Vector4d world (view_projection_matrix * world_pt);
// Normalize w-component
world /= world.w ();
// X/Y screen space coordinate
int screen_x = int (floor (double (((world.x () + 1) / 2.0) * width) + 0.5));
int screen_y = int (floor (double (((world.y () + 1) / 2.0) * height) + 0.5));
// Calculate -world_pt.y () because the screen Y axis is oriented top->down, ie 0 is top-left
//int winY = (int) floor ( (double) (((1 - world_pt.y ()) / 2.0) * height) + 0.5); // top left
return (Eigen::Vector2i (screen_x, screen_y));
}
示例5: cloudCallback
void cloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud)
{
if( ( !cloud_updated ) && ( goal_completion_time + ros::Duration(2.0) < cloud->header.stamp ) )
{
try
{
// update the pose
listener.waitForTransform( "/arm_kinect_frame", "/map", cloud->header.stamp, ros::Duration(5.0));
listener.lookupTransform( "/map", "/arm_kinect_frame", cloud->header.stamp, kinect2map);
listener.waitForTransform( "/arm_kinect_optical_frame", "/map", cloud->header.stamp, ros::Duration(5.0));
listener.lookupTransform( "/map", "/arm_kinect_optical_frame", cloud->header.stamp, optical2map);
tf::Vector3 position_( kinect2map.getOrigin() );
position.x() = position_.x();
position.y() = position_.y();
position.z() = position_.z();
tf::Quaternion orientation_( kinect2map.getRotation() );
orientation.x() = orientation_.x();
orientation.y() = orientation_.y();
orientation.z() = orientation_.z();
orientation.w() = orientation_.w();
ROS_INFO_STREAM("position = " << position.transpose() );
ROS_INFO_STREAM("orientation = " << orientation.transpose() );
// update the cloud
pcl::copyPointCloud(*cloud, *xyz_cld_ptr); // Do I need to copy it?
//xyz_cld_ptr = cloud;
cloud_updated = true;
}
catch (tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
}
}
}
示例6: 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>());
}
//.........这里部分代码省略.........