本文整理汇总了C++中Vector6d类的典型用法代码示例。如果您正苦于以下问题:C++ Vector6d类的具体用法?C++ Vector6d怎么用?C++ Vector6d使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Vector6d类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: logmap_se3
Vector6d logmap_se3(Matrix4d T){
Matrix3d R, Id3 = Matrix3d::Identity();
Vector3d Vt, t, w;
Matrix3d V = Matrix3d::Identity(), w_hat = Matrix3d::Zero();
Vector6d x;
Vt << T(0,3), T(1,3), T(2,3);
w << 0.f, 0.f, 0.f;
R = T.block(0,0,3,3);
double cosine = (R.trace() - 1.f)/2.f;
if(cosine > 1.f)
cosine = 1.f;
else if (cosine < -1.f)
cosine = -1.f;
double sine = sqrt(1.0-cosine*cosine);
if(sine > 1.f)
sine = 1.f;
else if (sine < -1.f)
sine = -1.f;
double theta = acos(cosine);
if( theta > 0.000001 ){
w_hat = theta*(R-R.transpose())/(2.f*sine);
w = skewcoords(w_hat);
Matrix3d s;
s = skew(w) / theta;
V = Id3 + s * (1.f-cosine) / theta + s * s * (theta - sine) / theta;
}
t = V.inverse() * Vt;
x.head(3) = t;
x.tail(3) = w;
return x;
}
开发者ID:rubengooj,项目名称:StVO-PL,代码行数:31,代码来源:auxiliar.cpp
示例2: gc_Rt_to_wt
Vector6d gc_Rt_to_wt( pose_t Rt ) {
Vector6d wt;
wt.head(3) = gc_Rodriguez( Rt.R );
wt.tail(3) = Rt.t;
return wt;
}
开发者ID:rgkoo,项目名称:slslam,代码行数:7,代码来源:gc.cpp
示例3: gc_asd_to_av
Vector6d gc_asd_to_av(Vector4d asd) {
Vector6d av;
Vector3d aa = asd.head(3);
// double d_inv = asd(3);
// double sig_d_inv = (1.0 - exp(-asd(3))) / (2.0 * (1.0 + exp(-asd(3))));
// double sig_d_inv = -log(1.0/asd(3) - 1.0);
// double sig_d_inv = log( (2.0 * asd(3) + 1.0) / (1.0 - 2.0*asd(3)) );
// double sig_d_inv = atan(asd(3)) / 2.0;
// double sig_d_inv = atan2(asd(3), 1.0) / 2.0;
// double sig_d_inv = atan2(asd(3), 1.0);
// double sig_d_inv = atan2(asd(3), 1.0) * 1.0;
// double sig_d_inv = tan(4.0 * asd(3));
double sig_d_inv = log(asd(3));
// cout << "sig_d_inv = " << sig_d_inv << endl;
// double sig_d_inv = cos(asd(3)) / sin(asd(3));
// double sig_d_inv = sin(asd(3)) / cos(asd(3));
// double sig_d_inv = sin(asd(3)) / cos(asd(3));
Matrix3d R = gc_Rodriguez(aa);
// av.head(3) = R.col(2) / sig_d_inv;
av.head(3) = R.col(2) * sig_d_inv;
av.tail(3) = R.col(0);
return av;
}
开发者ID:rgkoo,项目名称:slslam,代码行数:34,代码来源:gc.cpp
示例4: apply
static Vector6d apply(const Matrix4d &T, const Vector6d &q, double w) {
Vector6d q_t;
q_t << q.head(3), w, 0, 0;
q_t.head(4) = T * q_t.head(4);
q_t.tail(3) = q.tail(3);
return q_t;
}
开发者ID:jpanikulam,项目名称:software-common,代码行数:7,代码来源:C3Trajectory.cpp
示例5: mapCartesian
/*!
* This function is implemented in order to not use any external
* mapping functions. The function was build completely from pieces of
* code of the old cartesianStateDerivativeModel in order to use
* completely different, but also verified methods of constructing a
* stateDerivative from the total accelerations.
* \param currentState The current state.
* \param accelerations Total sum of accelerations.
* \return stateDerivative
*/
Vector6d mapCartesian( const Vector6d& cartesianState, const Eigen::Vector3d& acceleration )
{
// {{{ Snippets from cartesianStateDerivativeModel.h
typedef Vector6d CartesianStateDerivativeType;
// Declare Cartesian state derivative size.
unsigned int stateDerivativeSize = cartesianState.rows( );
// Declare Cartesian state derivative of the same size as Cartesian state.
CartesianStateDerivativeType cartesianStateDerivative
= CartesianStateDerivativeType::Zero( stateDerivativeSize );
// Set derivative of position components to current Cartesian velocity.
cartesianStateDerivative.segment( 0, stateDerivativeSize / 2 )
= cartesianState.segment( stateDerivativeSize / 2, stateDerivativeSize / 2 );
// Add transformed acceleration to state derivative.
cartesianStateDerivative.segment( stateDerivativeSize / 2, stateDerivativeSize / 2 )
+= acceleration;
// Return assembled state derivative.
return cartesianStateDerivative;
// }}} End Snippets from cartesianStateDerivativeModel.h
}
开发者ID:Haider-BA,项目名称:tudat,代码行数:36,代码来源:unitTestOrbitalStateDerivativeModel.cpp
示例6: computeTorques
/* ******************************************************************************************** */
void computeTorques (const Vector6d& state, double& ul, double& ur) {
// Set reference based on the mode
Vector6d refState;
if(locoMode == 1 || locoMode == 2) refState << 0.0, 0.0, state0(2), 0.0, state0(4), 0.0;
else {
ul = ur = 0.0;
return;
}
// Set the gains
Vector6d K;
if(locoMode == 1) K = K_stand;
else if(locoMode == 2) K = K_bal;
else assert(false);
if(dbg) cout << "K: " << K.transpose() << endl;
// Compute the error
Vector6d error = state - refState;
if(dbg) cout << "error: " << error.transpose() << endl;
// Compute the forward and spin torques
double u_x = K(2)*error(2) + K(3)*error(3);
double u_spin = K.bottomLeftCorner<2,1>().dot(error.bottomLeftCorner<2,1>());
double u_theta = K.topLeftCorner<2,1>().dot(error.topLeftCorner<2,1>());
// Limit the output torques
if(dbg) printf("u_theta: %lf, u_x: %lf, u_spin: %lf\n", u_theta, u_x, u_spin);
u_spin = max(-10.0, min(10.0, u_spin));
ul = u_theta + u_x + u_spin;
ur = u_theta + u_x - u_spin;
ul = max(-50.0, min(50.0, ul));
ur = max(-50.0, min(50.0, ur));
}
开发者ID:cerdogan,项目名称:iserFinal,代码行数:35,代码来源:05-standUp.cpp
示例7: gc_aid_to_av
Vector6d gc_aid_to_av(Vector4d aid) {
Vector6d av;
Vector3d aa = aid.head(3);
double d = 1.0 / aid(3);
Matrix3d R = gc_Rodriguez(aa);
av.head(3) = R.col(2) * d;
av.tail(3) = R.col(0);
// Vector6d av;
// double a = aid[0];
// double b = aid[1];
// double g = aid[2];
// double t = aid[3];
//
// double s1 = sin(a);
// double c1 = cos(a);
// double s2 = sin(b);
// double c2 = cos(b);
// double s3 = sin(g);
// double c3 = cos(g);
//
// Matrix3d R;
// R <<
// c2 * c3, s1 * s2 * c3 - c1 * s3, c1 * s2 * c3 + s1 * s3,
// c2 * s3, s1 * s2 * s3 + c1 * c3, c1 * s2 * s3 - s1 * c3,
// -s2, s1 * c2, c1 * c2;
//
// double d = 1.0 / t;
// av.head(3) = -R.col(2) * d;
// av.tail(3) = R.col(1);
return av;
}
开发者ID:rgkoo,项目名称:slslam,代码行数:34,代码来源:gc.cpp
示例8: gc_av_to_orth
Vector4d gc_av_to_orth(Vector6d av) {
Vector4d orth;
Vector3d a = av.head(3);
Vector3d v = av.tail(3); // v
Vector3d n = a.cross(v); // n
Vector3d x = n / n.norm();
Vector3d y = v / v.norm();
Vector3d z = x.cross(y);
orth[0] = atan2( y(2), z(2) );
orth[1] = asin( - x(2) );
orth[2] = atan2( x(1), x(0) );
Vector2d w( n.norm(), v.norm() );
w = w / w.norm();
orth[3] = asin( w(1) );
// MatrixXd A(3,2), Q, R;
//
// A.col(0) = n;
// A.col(1) = v;
//
// Eigen::FullPivHouseholderQR<MatrixXd> qr(A);
// // Q = qr.householderQ();
// Q = qr.matrixQ();
// R = qr.matrixQR().triangularView<Upper>();
// // std::cout << Q << "\n\n" << R << "\n\n" << Q * R - A << "\n";
//
// // double sigma1 = R(0,0);
// // double sigma2 = R(1,1);
//
// // cout << "\ntheta from sigma1 = " << acos(sigma1) << endl;
// // cout << "theta from sigma2 = " << asin(sigma2) << endl;
//
// // cout << "\nsigma1 = " << sigma1<< endl;
// // cout << "sigma2 = " << sigma2<< endl;
//
// // sigma2 /= sqrt(sigma1*sigma1 + sigma2*sigma2);
//
// Vector3d x = Q.col(0);
// Vector3d y = Q.col(1);
// Vector3d z = Q.col(2);
//
// orth[0] = atan2( y(2), z(2) );
// orth[1] = asin( - x(2) );
// orth[2] = atan2( x(1), x(0) );
// // orth[3] = asin(sigma2);
//
// Vector2d w( n.norm(), v.norm() );
// w = w / w.norm();
// orth[3] = asin( w(1) );
return orth;
}
开发者ID:rgkoo,项目名称:slslam,代码行数:57,代码来源:gc.cpp
示例9: gc_av_to_asd
Vector4d gc_av_to_asd(Vector6d av) {
Vector4d asd;
Vector3d a = av.head(3);
Vector3d x = av.tail(3); // v
Vector3d y = a.cross(x); // n
// Vector2d w(y.norm(), x.norm());
// w /= w.norm();
// asd(3) = asin(w(1));
double depth = x.norm() / y.norm();
// double sig_d = log( (2.0*depth + 1.0) / (1.0 - 2.0*depth));
// double quotient = depth / 0.5;
// int integer_quotient = (int)quotient;
// double floating_quotient = quotient - (double)integer_quotient;
// depth = depth * floating_quotient;
// double sig_d = log(2.0*depth + 1.0) - log(1.0 - 2.0*depth);
// double sig_d = atan(1.0 / (1.0*depth) );
// double sig_d = atan(depth);
// double sig_d = atan2(1.0, depth);
// double sig_d = atan(depth);
// double sig_d = atan2(1.0, depth) / 4.0;
double sig_d = 1.0 / exp(-depth);
asd(3) = sig_d;
// cout << "sig_d = " << sig_d << endl;
// asd(3) = depth;
// double sig_d = tan(1.0/depth);
// double sig_d = tan(2.0/depth);
// double sig_d = tan(2.0*depth);
// double sig_d = (1.0 - exp(-1.0/depth)) / (2.0*(1.0 + exp(-1.0/depth)));
// double sig_d = (1.0 - exp(-depth)) / (2.0*(1.0 + exp(-depth)));
// double sig_d = 1.0 / (1.0 + exp(-1.0/depth));
// double sig_d = 1.0 / (1.0 + exp(-depth));
x /= x.norm();
y /= y.norm();
Vector3d z = x.cross(y);
Matrix3d R;
R.col(0) = x;
R.col(1) = y;
R.col(2) = z;
Vector3d aa = gc_Rodriguez(R);
asd(0) = aa(0);
asd(1) = aa(1);
asd(2) = aa(2);
return asd;
}
开发者ID:rgkoo,项目名称:slslam,代码行数:57,代码来源:gc.cpp
示例10: SetToZero
void ForceSensor::voltage2FT() {
SetToZero(wrench_);
Vector6d result_voltage = voltage_ADC_ - bias_;
Vector6d force = conversion_matrix * result_voltage;
force = force.array() * conversion_scale.array();
for (int i = 0; i < 6; i++) {
wrench_[i] = force(i);
}
}
开发者ID:PatrykWasowski,项目名称:irp6_robot,代码行数:12,代码来源:ForceSensor.cpp
示例11: gc_plk_to_pose
Vector6d gc_plk_to_pose( Vector6d plk_w, pose_t T ) {
Vector3d nw = plk_w.head(3);
Vector3d vw = plk_w.tail(3);
Vector3d nc = T.R * nw + gc_skew_symmetric(T.t) * T.R * vw;
Vector3d vc = T.R * vw;
Vector6d plk_c;
plk_c.head(3) = nc;
plk_c.tail(3) = vc;
return plk_c;
}
开发者ID:rgkoo,项目名称:slslam,代码行数:12,代码来源:gc.cpp
示例12: motionSubspaceDotTimesV
void RollPitchYawFloatingJoint::motionSubspaceDotTimesV(const Eigen::Ref<const VectorXd>& q, const Eigen::Ref<const VectorXd>& v,
Vector6d& motion_subspace_dot_times_v,
Gradient<Vector6d, Eigen::Dynamic>::type* dmotion_subspace_dot_times_vdq,
Gradient<Vector6d, Eigen::Dynamic>::type* dmotion_subspace_dot_times_vdv) const
{
motion_subspace_dot_times_v.resize(TWIST_SIZE, 1);
auto rpy = q.middleRows<RPY_SIZE>(SPACE_DIMENSION);
double roll = rpy(0);
double pitch = rpy(1);
double yaw = rpy(2);
auto pd = v.middleRows<SPACE_DIMENSION>(0);
double xd = pd(0);
double yd = pd(1);
double zd = pd(2);
auto rpyd = v.middleRows<RPY_SIZE>(SPACE_DIMENSION);
double rolld = rpyd(0);
double pitchd = rpyd(1);
double yawd = rpyd(2);
using namespace std;
double cr = cos(roll);
double sr = sin(roll);
double cp = cos(pitch);
double sp = sin(pitch);
double cy = cos(yaw);
double sy = sin(yaw);
motion_subspace_dot_times_v.transpose() << -pitchd * yawd * cp, rolld * yawd * cp * cr - pitchd * yawd * sp * sr - pitchd * rolld * sr, -pitchd * rolld * cr - pitchd * yawd * cr * sp - rolld * yawd * cp * sr, yd * (yawd * cp * cy - pitchd * sp * sy) - xd * (pitchd * cy * sp + yawd * cp * sy)
- pitchd * zd * cp, zd * (rolld * cp * cr - pitchd * sp * sr) + xd * (rolld * (sr * sy + cr * cy * sp) - yawd * (cr * cy + sp * sr * sy) + pitchd * cp * cy * sr) - yd * (rolld * (cy * sr - cr * sp * sy) + yawd * (cr * sy - cy * sp * sr) - pitchd * cp * sr * sy), xd
* (rolld * (cr * sy - cy * sp * sr) + yawd * (cy * sr - cr * sp * sy) + pitchd * cp * cr * cy) - zd * (pitchd * cr * sp + rolld * cp * sr) + yd * (yawd * (sr * sy + cr * cy * sp) - rolld * (cr * cy + sp * sr * sy) + pitchd * cp * cr * sy);
if (dmotion_subspace_dot_times_vdq) {
dmotion_subspace_dot_times_vdq->resize(motion_subspace_dot_times_v.rows(), getNumPositions());
dmotion_subspace_dot_times_vdq->transpose() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -pitchd * rolld * cr - pitchd * yawd * cr * sp - rolld * yawd * cp * sr, pitchd * rolld * sr + pitchd * yawd * sp * sr - rolld * yawd * cp * cr, 0.0, xd
* (rolld * (cr * sy - cy * sp * sr) + yawd * (cy * sr - cr * sp * sy) + pitchd * cp * cr * cy) - zd * (pitchd * cr * sp + rolld * cp * sr) + yd * (-rolld * (cr * cy + sp * sr * sy) + yawd * (sr * sy + cr * cy * sp) + pitchd * cp * cr * sy), -zd * (rolld * cp * cr - pitchd * sp * sr)
- xd * (rolld * (sr * sy + cr * cy * sp) - yawd * (cr * cy + sp * sr * sy) + pitchd * cp * cy * sr) + yd * (rolld * (cy * sr - cr * sp * sy) + yawd * (cr * sy - cy * sp * sr) - pitchd * cp * sr * sy), pitchd * yawd * sp, -pitchd * yawd * cp * sr - rolld * yawd * cr * sp, rolld * yawd * sp
* sr - pitchd * yawd * cp * cr, -xd * (pitchd * cp * cy - yawd * sp * sy) - yd * (pitchd * cp * sy + yawd * cy * sp) + pitchd * zd * sp, -zd * (pitchd * cp * sr + rolld * cr * sp) - xd * (-rolld * cp * cr * cy + pitchd * cy * sp * sr + yawd * cp * sr * sy)
+ yd * (rolld * cp * cr * sy + yawd * cp * cy * sr - pitchd * sp * sr * sy), -zd * (pitchd * cp * cr - rolld * sp * sr) - xd * (pitchd * cr * cy * sp + rolld * cp * cy * sr + yawd * cp * cr * sy) - yd * (-yawd * cp * cr * cy + pitchd * cr * sp * sy + rolld * cp * sr * sy), 0.0, 0.0, 0.0, -xd
* (yawd * cp * cy - pitchd * sp * sy) - yd * (pitchd * cy * sp + yawd * cp * sy), yd * (rolld * (sr * sy + cr * cy * sp) - yawd * (cr * cy + sp * sr * sy) + pitchd * cp * cy * sr) + xd * (rolld * (cy * sr - cr * sp * sy) + yawd * (cr * sy - cy * sp * sr) - pitchd * cp * sr * sy), yd
* (rolld * (cr * sy - cy * sp * sr) + yawd * (cy * sr - cr * sp * sy) + pitchd * cp * cr * cy) - xd * (-rolld * (cr * cy + sp * sr * sy) + yawd * (sr * sy + cr * cy * sp) + pitchd * cp * cr * sy);
}
if (dmotion_subspace_dot_times_vdv) {
dmotion_subspace_dot_times_vdv->resize(motion_subspace_dot_times_v.rows(), getNumVelocities());
dmotion_subspace_dot_times_vdv->transpose() << 0.0, 0.0, 0.0, -pitchd * cy * sp - yawd * cp * sy, rolld * (sr * sy + cr * cy * sp) - yawd * (cr * cy + sp * sr * sy) + pitchd * cp * cy * sr, rolld * (cr * sy - cy * sp * sr) + yawd * (cy * sr - cr * sp * sy) + pitchd * cp * cr * cy, 0.0, 0.0, 0.0, yawd * cp
* cy - pitchd * sp * sy, -rolld * (cy * sr - cr * sp * sy) - yawd * (cr * sy - cy * sp * sr) + pitchd * cp * sr * sy, -rolld * (cr * cy + sp * sr * sy) + yawd * (sr * sy + cr * cy * sp) + pitchd * cp * cr * sy, 0.0, 0.0, 0.0, -pitchd * cp, rolld * cp * cr - pitchd * sp * sr, -pitchd * cr
* sp - rolld * cp * sr, 0.0, -pitchd * sr + yawd * cp * cr, -pitchd * cr - yawd * cp * sr, 0.0, xd * (sr * sy + cr * cy * sp) - yd * (cy * sr - cr * sp * sy) + zd * cp * cr, xd * (cr * sy - cy * sp * sr) - yd * (cr * cy + sp * sr * sy) - zd * cp * sr, -yawd * cp, -sr * (rolld + yawd * sp), -cr
* (rolld + yawd * sp), -zd * cp - xd * cy * sp - yd * sp * sy, sr * (-zd * sp + xd * cp * cy + yd * cp * sy), cr * (-zd * sp + xd * cp * cy + yd * cp * sy), -pitchd * cp, rolld * cp * cr - pitchd * sp * sr, -pitchd * cr * sp - rolld * cp * sr, cp * (yd * cy - xd * sy), -xd
* (cr * cy + sp * sr * sy) - yd * (cr * sy - cy * sp * sr), xd * (cy * sr - cr * sp * sy) + yd * (sr * sy + cr * cy * sp);
}
}
开发者ID:AkshayBabbar,项目名称:drake,代码行数:53,代码来源:RollPitchYawFloatingJoint.cpp
示例13: motionSubspaceDotTimesV
void QuaternionFloatingJoint::motionSubspaceDotTimesV(const Eigen::Ref<const VectorXd>& q, const Eigen::Ref<const VectorXd>& v,
Vector6d& motion_subspace_dot_times_v,
Gradient<Vector6d, Eigen::Dynamic>::type* dmotion_subspace_dot_times_vdq,
Gradient<Vector6d, Eigen::Dynamic>::type* dmotion_subspace_dot_times_vdv) const
{
motion_subspace_dot_times_v.setZero();
if (dmotion_subspace_dot_times_vdq) {
dmotion_subspace_dot_times_vdq->setZero(motion_subspace_dot_times_v.size(), getNumPositions());
}
if (dmotion_subspace_dot_times_vdv) {
dmotion_subspace_dot_times_vdv->setZero(motion_subspace_dot_times_v.size(), getNumVelocities());
}
}
开发者ID:lessc0de,项目名称:drake,代码行数:13,代码来源:QuaternionFloatingJoint.cpp
示例14: run
/// The main loop
void run() {
// start some timers
Vector6d state;
size_t c_ = 0;
struct timespec t_now, t_prev = aa_tm_now();
int lastLocoMode = locoMode;
while(!somatic_sig_received) {
pthread_mutex_lock(&mutex);
dbg = (c_++ % 20 == 0);
if(dbg) cout << "\nmode: " << locoMode << endl;
// Update times
t_now = aa_tm_now();
double dt = (double)aa_tm_timespec2sec(aa_tm_sub(t_now, t_prev));
t_prev = t_now;
if(dbg) cout << "dt: " << dt << endl;
// Get the locomotion state
getState(state, dt);
if(dbg) cout << "state: " << state.transpose() << endl;
// Check if the arm has reached the goal position or if the body is moving
bool armReached = (fabs(hw->arms[LEFT]->pos[0] - (-1.8)) < 0.05);
bool baseMoved = (hw->imu > -1.8);
if(dbg) cout << "armReached: " << armReached << ", baseMoved: " << baseMoved << endl;
if((armReached || baseMoved) && (locoMode == 0)) {
locoMode = 1;
state0 = state;
}
if(dbg) cout << "state0: " << state0.transpose() << endl;
// Switch the mode if necessary
switchModes(state);
// Compute the torques based on the state and the mode
double ul, ur;
computeTorques(state, ul, ur);
// Apply the torque
double input [2] = {ul, ur};
if(dbg) cout << "start: " << start << "\nu: {" << ul << ", " << ur << "}" << endl;
if(!start) input[0] = input[1] = 0.0;
// somatic_motor_cmd(&daemon_cx, hw->amc, SOMATIC__MOTOR_PARAM__MOTOR_CURRENT, input, 2, NULL);
// Update the locomotion mode
lastLocoMode = locoMode;
pthread_mutex_unlock(&mutex);
}
}
开发者ID:cerdogan,项目名称:iserFinal,代码行数:52,代码来源:05-standUp.cpp
示例15: gc_line_to_pose
Vector6d gc_line_to_pose(Vector6d line_w, pose_t T) {
Vector6d line_c;
Vector3d cp0, dv0;
cp0 = line_w.head(3);
dv0 = line_w.tail(3);
Vector3d cp1 = gc_point_to_pose( T, cp0 );
Vector3d dv1 = T.R * dv0;
line_c.head(3) = cp1;
line_c.tail(3) = dv1;
return line_c;
}
开发者ID:rgkoo,项目名称:slslam,代码行数:15,代码来源:gc.cpp
示例16: pp
template <typename PointSource, typename PointTarget> inline void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::fdf (const Vector6d& x, double& f, Vector6d& g)
{
Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
gicp_->applyState(transformation_matrix, x);
f = 0;
g.setZero ();
Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
const int m = static_cast<const int> (gicp_->tmp_idx_src_->size ());
for (int i = 0; i < m; ++i)
{
// The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
// The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
Eigen::Vector4f pp (transformation_matrix * p_src);
// The last coordiante is still guaranteed to be set to 1.0
Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
// temp = M*res
Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
// Increment total error
f+= double(res.transpose() * temp);
// Increment translation gradient
// g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
g.head<3> ()+= temp;
pp = gicp_->base_transformation_ * p_src;
Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
// Increment rotation gradient
R+= p_src3 * temp.transpose();
}
f/= double(m);
g.head<3> ()*= double(2.0/m);
R*= 2.0/m;
gicp_->computeRDerivative(x, R, g);
}
开发者ID:87west,项目名称:pcl,代码行数:35,代码来源:gicp.hpp
示例17:
pair<Matrix4d, Matrix4d> C3Trajectory::transformation_pair(const Vector6d &q) {
Matrix4d R;
R.block<3,3>(0, 0) = AttitudeHelpers::EulerToRotation(q.tail(3));
R.block<1,3>(3, 0).fill(0);
R.block<3,1>(0, 3).fill(0);
R(3, 3) = 1;
Matrix4d T = Matrix4d::Identity();
T.block<3,1>(0, 3) = -q.head(3);
pair<Matrix4d, Matrix4d> result;
result.first = R.transpose()*T; // NED -> BODY
T.block<3,1>(0, 3) = q.head(3);
result.second = T*R; // BODY -> NED
return result;
}
开发者ID:jpanikulam,项目名称:software-common,代码行数:18,代码来源:C3Trajectory.cpp
示例18: expmap_se3
Matrix4d expmap_se3(Vector6d x){
Matrix3d R, V, s, I = Matrix3d::Identity();
Vector3d t, w;
Matrix4d T = Matrix4d::Identity();
w = x.tail(3);
t = x.head(3);
double theta = w.norm();
if( theta < 0.000001 )
R = I;
else{
s = skew(w)/theta;
R = I + s * sin(theta) + s * s * (1.0f-cos(theta));
V = I + s * (1.0f - cos(theta)) / theta + s * s * (theta - sin(theta)) / theta;
t = V * t;
}
T.block(0,0,3,4) << R, t;
return T;
}
开发者ID:rubengooj,项目名称:StVO-PL,代码行数:18,代码来源:auxiliar.cpp
示例19: determinePartCoefficients
//! Determine aerodynamic coefficients of a single vehicle part.
Vector6d HypersonicLocalInclinationAnalysis::determinePartCoefficients(
const int partNumber, const boost::array< int, 3 > independentVariableIndices )
{
// Declare and determine angles of attack and sideslip for analysis.
double angleOfAttack = dataPointsOfIndependentVariables_[ 1 ]
[ independentVariableIndices[ 1 ] ];
double angleOfSideslip = dataPointsOfIndependentVariables_[ 2 ]
[ independentVariableIndices[ 2 ] ];
// Declare partCoefficient vector.
Vector6d partCoefficients = Vector6d::Zero( );
// Check whether the inclinations of the vehicle part have already been computed.
if ( previouslyComputedInclinations_.count( std::pair< double, double >(
angleOfAttack, angleOfSideslip ) ) == 0 )
{
// Determine panel inclinations for part.
determineInclinations( angleOfAttack, angleOfSideslip );
// Add panel inclinations to container
previouslyComputedInclinations_[ std::pair< double, double >(
angleOfAttack, angleOfSideslip ) ] = inclination_;
}
else
{
// Fetch inclinations from container
inclination_ = previouslyComputedInclinations_[ std::pair< double, double >(
angleOfAttack, angleOfSideslip ) ];
}
// Set pressureCoefficient_ array for given independent variables.
determinePressureCoefficients( partNumber, independentVariableIndices );
// Calculate force coefficients from pressure coefficients.
partCoefficients.segment( 0, 3 ) = calculateForceCoefficients( partNumber );
// Calculate moment coefficients from pressure coefficients.
partCoefficients.segment( 3, 3 ) = calculateMomentCoefficients( partNumber );
return partCoefficients;
}
开发者ID:JPelamatti,项目名称:TudatDevelopment,代码行数:44,代码来源:hypersonicLocalInclinationAnalysis.cpp
示例20: readGains
/// Read file for gains
void readGains () {
// Get the gains
Vector6d* kgains [] = {&K_stand, &K_bal};
ifstream file ("/home/cerdogan/Documents/Software/project/krang/iser/data/gains-03.txt");
assert(file.is_open());
char line [1024];
for(size_t k_idx = 0; k_idx < 2; k_idx++) {
*kgains[k_idx] = Vector6d::Zero();
file.getline(line, 1024);
std::stringstream stream(line, std::stringstream::in);
size_t i = 0;
double newDouble;
while ((i < 6) && (stream >> newDouble)) (*kgains[k_idx])(i++) = newDouble;
}
cout << "K_stand: " << K_stand.transpose() << endl;
cout << "K_bal: " << K_bal.transpose() << endl;
file.close();
}
开发者ID:cerdogan,项目名称:iserFinal,代码行数:20,代码来源:05-standUp.cpp
注:本文中的Vector6d类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论