本文整理汇总了C++中eigen::Vector4d类的典型用法代码示例。如果您正苦于以下问题:C++ Vector4d类的具体用法?C++ Vector4d怎么用?C++ Vector4d使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Vector4d类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: projectPoints
void ObjectModelLine::projectPoints (const std::vector<int> &inliers, const Eigen::VectorXd &model_coefficients,
PointCloud3D* projectedPointCloud){
assert (model_coefficients.size () == 6);
// Obtain the line point and direction
Eigen::Vector4d line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4d line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
// Iterate through the 3d points and calculate the distances from them to the line
for (size_t i = 0; i < inliers.size (); ++i)
{
Eigen::Vector4d pt ((*inputPointCloud->getPointCloud())[inliers[i]].getX(), (*inputPointCloud->getPointCloud())[inliers[i]].getY(),
(*inputPointCloud->getPointCloud())[inliers[i]].getZ(), 0);
// double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
double k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
Eigen::Vector4d pp = line_pt + k * line_dir;
// Calculate the projection of the point on the line (pointProj = A + k * B)
// std::vector<Point3D> *projectedPoints = projectedPointCloud->getPointCloud();
(*projectedPointCloud->getPointCloud())[i].setX(pp[0]);
(*projectedPointCloud->getPointCloud())[i].setY(pp[1]);
(*projectedPointCloud->getPointCloud())[i].setZ(pp[2]);
}
}
开发者ID:brics,项目名称:brics_3d,代码行数:27,代码来源:ObjectModelLine.cpp
示例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();
}
开发者ID:EricLYang,项目名称:fovis,代码行数:7,代码来源:stereo_disparity.cpp
示例3: transformationFromPropertyTree
inline sm::kinematics::Transformation transformationFromPropertyTree(const sm::ConstPropertyTree & config)
{
Eigen::Vector4d q = sm::eigen::quaternionFromPropertyTree( sm::ConstPropertyTree(config, "q_a_b") );
q = q / q.norm();
Eigen::Vector3d t = sm::eigen::vector3FromPropertyTree( sm::ConstPropertyTree(config, "t_a_b_a" ) );
return sm::kinematics::Transformation(q,t);
}
开发者ID:ethz-asl,项目名称:Schweizer-Messer,代码行数:7,代码来源:property_tree.hpp
示例4: _IterateCurvatureReduction
void BezierBoundarySolver::_IterateCurvatureReduction(BezierBoundaryProblem* pProblem,Eigen::Vector4d& params)
{
double epsilon = 0.0001;
//create a jacobian for the parameters by perturbing them
Eigen::Vector4d Jt; //transpose of the jacobian
BezierBoundaryProblem origProblem = *pProblem;
double maxK = _GetMaximumCurvature(pProblem);
for(int ii = 0; ii < 4 ; ii++){
Eigen::Vector4d epsilonParams = params;
epsilonParams[ii] += epsilon;
_Get5thOrderBezier(pProblem,epsilonParams);
_Sample5thOrderBezier(pProblem);
double kPlus = _GetMaximumCurvature(pProblem);
epsilonParams[ii] -= 2*epsilon;
_Get5thOrderBezier(pProblem,epsilonParams);
_Sample5thOrderBezier(pProblem);
double kMinus = _GetMaximumCurvature(pProblem);
Jt[ii] = (kPlus-kMinus)/(2*epsilon);
}
//now that we have Jt, we can calculate JtJ
Eigen::Matrix4d JtJ = Jt*Jt.transpose();
//thikonov regularization
JtJ += Eigen::Matrix4d::Identity();
Eigen::Vector4d deltaParams = JtJ.inverse() * Jt*maxK;
params -= deltaParams;
_Get5thOrderBezier(pProblem,params);
_Sample5thOrderBezier(pProblem);
//double finalMaxK = _GetMaximumCurvature(pProblem);
//dout("2D Iteration took k from " << maxK << " to " << finalMaxK);
}
开发者ID:crheckman,项目名称:CarPlanner,代码行数:34,代码来源:BezierBoundarySolver.cpp
示例5: qprod
static void qprod(const Eigen::Vector4d & q, const Eigen::Vector4d & p, Eigen::Vector4d & prod_result) {
double a=q(0);
Eigen::Vector3d v = q.segment(1, 3); //coefficients q
double x=p(0);
Eigen::Vector3d u = p.segment(1, 3); //coefficients p
prod_result << a*x-v.transpose()*u, (a*u+x*v) + v.cross(u);
}
开发者ID:Yvaine,项目名称:ekfoa,代码行数:8,代码来源:motion_model.hpp
示例6: pseudoLog_
void ExpMapQuaternion::pseudoLog_(RefVec out, const ConstRefVec& x, const ConstRefVec& y)
{
Eigen::Vector4d tmp;
toQuat q(tmp.data());
const toConstQuat xQ(x.data());
const toConstQuat yQ(y.data());
q = xQ.inverse()*yQ; //TODO double-check that formula
logarithm(out,tmp);
}
开发者ID:fdarricau,项目名称:manifolds,代码行数:9,代码来源:ExpMapQuaternion.cpp
示例7: keypoint
Eigen::Vector3d PointcloudProc::projectPoint(Eigen::Vector4d& point, CamParams cam) const
{
Eigen::Vector3d keypoint;
keypoint(0) = (cam.fx*point.x()) / point.z() + cam.cx;
keypoint(1) = (cam.fy*point.y()) / point.z() + cam.cy;
keypoint(2) = (cam.fx*(point.x()-cam.tx)/point.z() + cam.cx);
return keypoint;
}
开发者ID:RoboWGT,项目名称:robo_groovy,代码行数:10,代码来源:frame.cpp
示例8: intersectRayPlane
Eigen::Vector3d intersectRayPlane(const Eigen::Vector3d ray,
const Eigen::Vector3d ray_origin, const Eigen::Vector4d plane)
{
// Plane defined as ax + by + cz + d = 0
// Ray: P = P0 + tV
// Plane: P.N + d = 0, where P is intersection point
// t = -(P0.N + d)/(V.N) , P = P0 + tV
float t = -(ray_origin.dot(plane.head(3)) + plane[3]) / (ray.dot(plane.head(3)));
Eigen::Vector3d P = ray_origin + t*ray;
return P;
}
开发者ID:contradict,项目名称:SampleReturn,代码行数:11,代码来源:camera_ray.cpp
示例9: randomUnitQuaternion
void randomUnitQuaternion(Eigen::Vector4d &quat) {
static boost::random::mt19937 rng(time(NULL));
static boost::random::normal_distribution<> normal;
do {
quat(0) = normal(rng);
quat(1) = normal(rng);
quat(2) = normal(rng);
quat(3) = normal(rng);
} while (quat.norm() < 0.00001);
quat.normalize();
}
开发者ID:dseredyn,项目名称:planer_utils,代码行数:12,代码来源:random_uniform.cpp
示例10: getTransformationDelta
const double CMiniVisionToolbox::getTransformationDelta( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo )
{
//ds compute pose change
const Eigen::Isometry3d matTransformChange( p_matTransformationTo*p_matTransformationFrom.inverse( ) );
//ds check point
const Eigen::Vector4d vecSamplePoint( 40.2, -1.25, 2.5, 1.0 );
double dNorm( vecSamplePoint.norm( ) );
const Eigen::Vector4d vecDifference( vecSamplePoint-matTransformChange*vecSamplePoint );
dNorm = ( dNorm + vecDifference.norm( ) )/2;
//ds return norm
return ( std::fabs( vecDifference(0) ) + std::fabs( vecDifference(1) ) + std::fabs( vecDifference(2) ) )/dNorm;
}
开发者ID:schdomin,项目名称:vi_mapper,代码行数:14,代码来源:CMiniVisionToolbox.cpp
示例11: checkMovementThreshold
bool WHeadPositionCorrection::checkMovementThreshold( const WLEMDHPI::TransformationT& trans )
{
const Eigen::Vector4d diffTrans = m_transExc.data().col( 3 ) - trans.data().col( 3 );
if( diffTrans.norm() > m_movementThreshold )
{
m_transExc = trans;
return true;
}
// TODO(pieloth): check rotation, e.g. with 1 or more key points
// initial: pick 6 key points from dipoles min/max (x,0,0); (0,y,0); (0,0,z)
// compare max distance
// keyPoint 3x6
// (trans * keyPoints).colwise().norm() - (m_transExc * keyPoints).colwise().norm()).maxCoeff(), mind homog. coord.
return false;
}
开发者ID:labp,项目名称:na-online_ow-toolbox,代码行数:15,代码来源:WHeadPositionCorrection.cpp
示例12: vec
mathtools::geometry::euclidian::Line<4> skeleton::model::Perspective::toObj(
const Eigen::Matrix<double,skeleton::model::meta<skeleton::model::Projective>::stordim,1> &vec,
const mathtools::geometry::euclidian::Line<4> &) const
{
Eigen::Vector4d origin;
origin.block<3,1>(0,0) = m_frame3->getOrigin();
origin(3) = 0.0;
Eigen::Vector4d vecdir;
vecdir.block<3,1>(0,0) = m_frame3->getBasis()->getMatrix()*Eigen::Vector3d(vec(0),vec(1),1.0);
vecdir(3) = vec(2);
vecdir.normalize();
return mathtools::geometry::euclidian::Line<4>(origin,vecdir);
}
开发者ID:Ibujah,项目名称:skeletonbasedreconstruction,代码行数:15,代码来源:Perspective.cpp
示例13:
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();
}
开发者ID:LiliMeng,项目名称:fovis-1,代码行数:15,代码来源:visualization.cpp
示例14: motors_cb
void Node::motors_cb(const controller::Motors::ConstPtr& msg)
{
//get quad charectaristics
float d = msg->d;
float kf = msg->kf;
float kt = msg->kt;
//get motor speeds (rad/s)
const Eigen::Vector4d motor_speeds(
msg->motor1,
msg->motor2,
msg->motor3,
msg->motor4);
//calculate forces and moments
const Eigen::Vector4d motor_thrust = kf * motor_speeds.cwiseProduct(motor_speeds);
Eigen::Matrix4d convert;
convert << 1, 1, 1, 1,
0, d, 0, -d,
-d, 0, d, 0,
kt, -kt, kt, -kt;
const Eigen::Vector4d f_moments = convert * motor_thrust;
//publish message
geometry_msgs::Twist out;
out.linear.z = f_moments[0];
out.angular.x = f_moments[1];
out.angular.y = f_moments[2];
out.angular.z = f_moments[3];
force_pub_.publish(out);
}
开发者ID:anuragmakineni,项目名称:simulator,代码行数:34,代码来源:motors.cpp
示例15: 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));
}
开发者ID:2php,项目名称:pcl,代码行数:17,代码来源:common.cpp
示例16: sampler
std::unique_ptr<unsigned char[]> getRaster() {
const unsigned int row_bytes = scene.cam.viewportWidth * 3;
const unsigned int buf_len = scene.cam.viewportHeight * row_bytes;
auto buf = std::unique_ptr<unsigned char[]>(new unsigned char[buf_len]);
const double multFactor = 1 / (1.0 * NUM_AA_SUBPIXELS);
const Eigen::Vector4d maxrgb(255, 255, 255, 0);
tbb::parallel_for(
tbb::blocked_range2d<int, int>(0, scene.cam.viewportHeight, 0,
scene.cam.viewportWidth),
[&](tbb::blocked_range2d<int, int> &r) {
// Support for AA sampler
std::vector<Ray, Eigen::aligned_allocator<Ray>> rays(
NUM_AA_SUBPIXELS, Ray(Eigen::Vector4d(0, 0, 0, 1),
Eigen::Vector4d(1, 0, 0, 0), ID_AIR));
RandomAASampler sampler(scene.cam);
for (int row = r.rows().begin(); row != r.rows().end(); ++row) {
for (int col = r.cols().begin(); col != r.cols().end(); ++col) {
Camera cam(scene.cam);
Eigen::Vector4d color = Eigen::Vector4d::Zero();
// materials stack for refraction
std::array<int, MAX_DEPTH + 1> objStack;
if (!aa) {
Ray ray = cam.constructRayThroughPixel(col, row);
objStack.fill(ID_AIR);
color = getColor(ray, 0, 0, objStack);
} else {
sampler.constructRaysThroughPixel(col, row, rays);
for (int i = 0; i < NUM_AA_SUBPIXELS; ++i) {
objStack.fill(ID_AIR);
color += getColor(rays[i], 0, 0, objStack);
}
color *= multFactor;
}
color = 255 * color;
color = color.cwiseMin(maxrgb);
buf[row * row_bytes + (col * 3)] = color[0];
buf[row * row_bytes + (col * 3) + 1] = color[1];
buf[row * row_bytes + (col * 3) + 2] = color[2];
}
}
});
return buf;
}
开发者ID:drewbarbs,项目名称:class-raytracer,代码行数:46,代码来源:rayTracer.cpp
示例17: selectWithinDistance
void ObjectModelCylinder::selectWithinDistance (const Eigen::VectorXd &model_coefficients, double threshold,
std::vector<int> &inliers){
assert (model_coefficients.size () == 7);
int nr_p = 0;
inliers.resize (this->inputPointCloud->getSize());
Eigen::Vector4d line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4d line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
double ptdotdir = line_pt.dot (line_dir);
double dirdotdir = 1.0 / line_dir.dot (line_dir);
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < this->inputPointCloud->getSize(); ++i)
{
// Aproximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
Eigen::Vector4d pt = Eigen::Vector4d ((*inputPointCloud->getPointCloud())[i].getX(),
(*inputPointCloud->getPointCloud())[i].getY(),
(*inputPointCloud->getPointCloud())[i].getZ(), 0);
Eigen::Vector4d n = Eigen::Vector4d (this->normals->getNormals()->data()[i].getX(),
this->normals->getNormals()->data()[i].getY(),
this->normals->getNormals()->data()[i].getZ(), 0);
double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
double k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
Eigen::Vector4d pt_proj = line_pt + k * line_dir;
Eigen::Vector4d dir = pt - pt_proj;
dir.normalize ();
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, dir));
d_normal = fmin (d_normal, M_PI - d_normal);
if (fabs (this->normalDistanceWeight * d_normal + (1 - this->normalDistanceWeight) * d_euclid) < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = i;
nr_p++;
}
}
inliers.resize (nr_p);
}
开发者ID:brics,项目名称:brics_3d,代码行数:45,代码来源:ObjectModelCylinder.cpp
示例18: matSVD
const CPoint3DCAMERA CMiniVisionToolbox::getPointStereoLinearTriangulationSVDDLT( const cv::Point2d& p_ptPointLEFT, const cv::Point2d& p_ptPointRIGHT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionLEFT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionRIGHT )
{
//ds A matrix for system: A*X=0
Eigen::Matrix4d matAHomogeneous;
matAHomogeneous.row(0) = p_ptPointLEFT.x*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(0);
matAHomogeneous.row(1) = p_ptPointLEFT.y*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(1);
matAHomogeneous.row(2) = p_ptPointRIGHT.x*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(0);
matAHomogeneous.row(3) = p_ptPointRIGHT.y*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(1);
//ds solve system subject to ||A*x||=0 ||x||=1
const Eigen::JacobiSVD< Eigen::Matrix4d > matSVD( matAHomogeneous, Eigen::ComputeFullU | Eigen::ComputeFullV );
//ds solution x is the last column of V
const Eigen::Vector4d vecX( matSVD.matrixV( ).col( 3 ) );
return vecX.head( 3 )/vecX(3);
}
开发者ID:schdomin,项目名称:vi_mapper,代码行数:18,代码来源:CMiniVisionToolbox.cpp
示例19: 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;
}
开发者ID:AIS-Bonn,项目名称:humanoid_op_ros,代码行数:19,代码来源:planefit.cpp
示例20: uhp
TEST(UncertainHomogeneousPointTestSuite, testUav)
{
try {
using namespace sm::kinematics;
Eigen::Vector4d p;
p.setRandom();
p = p/p.norm();
Eigen::Matrix3d U;
U = sm::eigen::randomCovariance<3>();
UncertainHomogeneousPoint uhp(p,U);
sm::eigen::assertNear(U, uhp.U_av_form(), 1e-10, SM_SOURCE_FILE_POS, "Checking that I can recover the av/form point uncertainty");
}
catch(std::exception const & e)
{
FAIL() << e.what();
}
}
开发者ID:AliAlawieh,项目名称:kalibr,代码行数:19,代码来源:UncertainHomogeneousPoint.cpp
注:本文中的eigen::Vector4d类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论