コード例 #1
0
Matrix3d computeTform(
    const Matrix2Xd& pointsFrom, const Matrix2Xd& pointsTo,
    const VectorXd& indices ) {
  Matrix3d normMatFrom;
  Matrix3d normMatTo;
  Matrix3d T = computeSimilarity(
    normalizePoints( pointsFrom, indices, normMatFrom ),
    normalizePoints( pointsTo, indices, normMatTo ) );
  return normMatFrom.transpose() * ( T * normMatTo.transpose().inverse() );
}
コード例 #2
0
        // Original code from the ROS vslam package pe3d.cpp
        // uses the SVD procedure for aligning point clouds
        //   SEE: Arun, Huang, Blostein: Least-Squares Fitting of Two 3D Point Sets
        Eigen::Matrix4d threePointSvd(Eigen::MatrixXd const & p0, Eigen::MatrixXd const & p1)
        {
            using namespace Eigen;

            SM_ASSERT_EQ_DBG(std::runtime_error, p0.rows(), 3, "p0 must be a 3xK matrix");
            SM_ASSERT_EQ_DBG(std::runtime_error, p1.rows(), 3, "p1 must be a 3xK matrix");
            SM_ASSERT_EQ_DBG(std::runtime_error, p0.cols(), p1.cols(), "p0 and p1 must have the same number of columns");

            Vector3d c0 = p0.rowwise().mean();
            Vector3d c1 = p1.rowwise().mean();

            Matrix3d H(Matrix3d::Zero());
            // subtract out
            // p0a -= c0;
            // p0b -= c0;
            // p0c -= c0;
            // p1a -= c1;
            // p1b -= c1;
            // p1c -= c1;

            // Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
            // 	p1c*p0c.transpose();
            for(int i = 0; i < p0.cols(); ++i)
            {
                H += (p0.col(i) - c0) * (p1.col(i) - c1).transpose();
            }
      

            // do the SVD thang
            JacobiSVD<Matrix3d> svd(H,ComputeFullU | ComputeFullV);
            Matrix3d V = svd.matrixV();
            Matrix3d R = V * svd.matrixU().transpose();          
            double det = R.determinant();
        
            if (det < 0.0)
            {
                V.col(2) = V.col(2) * -1.0;
                R = V * svd.matrixU().transpose();
            }
            Vector3d tr = c0-R.transpose()*c1;    // translation 
      
            // transformation matrix, 3x4
            Matrix4d tfm(Matrix4d::Identity());
            //        tfm.block<3,3>(0,0) = R.transpose();
            //        tfm.col(3) = -R.transpose()*tr;
            tfm.topLeftCorner<3,3>() = R.transpose();
            tfm.topRightCorner<3,1>() = tr;
      
            return tfm;
      
        }
コード例 #3
0
ファイル: conics.cpp プロジェクト: stevenlovegrove/fiducials
boost::array<pair<Vector3d,Matrix3d >, 2 > PlaneFromConic(const Conic& c, double plane_circle_radius, const Matrix3d& K )
{
  const Matrix3d Cn = K.transpose() * c.C *K;
  Eigen::JacobiSVD<Matrix3d> svd(Cn, ComputeFullU );
  const Matrix3d R1 = svd.matrixU();
  const Vector3d l = svd.singularValues();
  const double t = atan( sqrt( (l[1]-l[0]) / (l[2]-l[1]) ) );

  boost::array<pair<Vector3d,Matrix3d >, 2> r;

  for( int i=0; i < 2; ++i )
  {
    const double theta = (i*2-1)*t;
    const Matrix3d R2 = RotY(theta);
    const Matrix3d R = R1 * R2;
//    const Matrix3d C__ = R.T() * Cn * R;
    const Vector3d n = R * Vector3d(0,0,-1);
    const double d = sqrt( (l[1]*l[1])/(l[0]*l[2]) ) * plane_circle_radius;

    r[i].first = n.normalized() / d;
    r[i].second = R;
  }

  return r;
}
コード例 #4
0
ファイル: MSCKF.cpp プロジェクト: SkyRiderMike/my_VINS
Vector2d MSCKF::projectPoint(Vector3d feature_pose, Matrix3d R_gb, Vector3d p_gb, Vector3d p_cb)
{
    Vector2d zij;
    zij = cam.h(R_cb * R_gb.transpose() * (feature_pose - p_gb) + p_cb);
    
    return zij;
}
コード例 #5
0
ファイル: auxiliar.cpp プロジェクト: rubengooj/StVO-PL
Matrix4d inverse_se3(Matrix4d T){
    Matrix4d Tinv = Matrix4d::Identity();
    Matrix3d R;
    Vector3d t;
    t = T.block(0,3,3,1);
    R = T.block(0,0,3,3);
    Tinv.block(0,0,3,3) =  R.transpose();
    Tinv.block(0,3,3,1) = -R.transpose() * t;
    return Tinv;
}
コード例 #6
0
ファイル: BodyNode.cpp プロジェクト: Tarrasch/dart
    void BodyNode::updateTransform() {
        mT = mJointParent->getLocalTransform();
        if (mNodeParent) mW = mNodeParent->mW * mT;
        else mW = mT;

        // update the inertia matrix 
        Matrix3d R = mW.topLeftCorner(3,3);
        if(mVizShape!=NULL)
            mIc = R*mI*R.transpose();
    }
コード例 #7
0
 RigidBodyInertia operator*(const Rotation& M,const RigidBodyInertia& I){
     //mb=ma
     //hb=R*h
     //Ib = R(Ia)R' with r=0
     Matrix3d R = Map<Matrix3d>(M.data);
     RotationalInertia Ib;
     Map<Matrix3d>(Ib.data) = R.transpose()*(Map<Matrix3d>(I.I.data)*R);
     
     return RigidBodyInertia(I.m,M*I.h,Ib,mhi);
 }
コード例 #8
0
double angle_mismatch(const Matrix3d& Q, const Matrix3d& R)
{
  //std::cout << "Q\n" << Q << "\nR\n" << R << std::endl;
  Matrix3d S = Q.transpose()*R;
  //std::cout << "S\n: " << S << std::endl;
  double thecos = (S.trace()-1.0)/2.0;
  thecos = std::max( std::min ( thecos, 1.0), -1.0);
  //std::cout << "thecos: " << thecos << " leads to angle: " << acos(thecos) << " * " << ( S(1,2) < 0.0 ? -1.0 : 1.0) << std::endl;

  return (acos(thecos) * ( S(1,2) < 0.0 ? -1.0 : 1.0));
}
コード例 #9
0
const Matrix3d VectorMath::TMatrix(const Vector3d &v)
{
    double vnormsq = v.dot(v);
    Matrix3d I;
    I.setIdentity();
    if(vnormsq == 0)
        return I;

    Matrix3d R = rotationMatrix(v);
    return (v*v.transpose() + (R.transpose()-I)*crossProductMatrix(v))/vnormsq;
}
コード例 #10
0
ファイル: conics.cpp プロジェクト: stevenlovegrove/fiducials
double Cost( const pair<Vector3d,Matrix3d >& plane, const Conic& conic, const Matrix3d& K )
{
  const Matrix3d Cn = K.transpose() * conic.C *K;
  const Matrix3d& R = plane.second;
  const Matrix3d C_ = R.transpose() * Cn * R;
  const double a = C_(0,0);
//  const double b = C_[0][1] / 2.0;
  const double c = C_(1,1);
  const double amc = abs(a - c);
  return amc / max(abs(a),abs(c));;// + b*b;
}
コード例 #11
0
void force_feedback_pos_rate_control(void){
  
  // ****************************************************************************************************************
  // Force feedback when in rate control area
  // ****************************************************************************************************************
  
  // Variables
  Ks = MatrixXd::Zero(3,3);
  Ks(0,0) = scale[0];
  Ks(1,1) = scale[1];
  Ks(2,2) = scale[2];
 
  Eigen::Vector3d omni_force = Eigen::Vector3d::Zero();
  double R = 0.05;
  double Fmax = 3.3; // 3.3 N es la máxima força nominal
  double Kf = 60;
  double Kv = 0.5;

  // Displacement
  dm = pm_i - pm_0;
  ds = Ks * Rs.transpose() * Rm * dm;  
  
  // Omni feedback force computation
  double D = sqrt( ds(0)*ds(0) + ds(1)*ds(1) );  
  
  if (D > R)	omni_force = - Kf * (D - R) * (ds/D);
  omni_force += - Kv*vm_i; // Damping
  omni_force(2) = 0.0;     // No force in vertical axis
  omni_force = Rm.transpose() * Rs * omni_force; // To master reference frame
  
  if (omni_force(0) < -Fmax) omni_force(0) = -Fmax;
  if (omni_force(0) > Fmax)  omni_force(0) = Fmax;
  if (omni_force(1) < -Fmax) omni_force(1) = -Fmax;
  if (omni_force(1) > Fmax)  omni_force(1) = Fmax;  

  // Publish
  force.x = omni_force(0);
  force.y = omni_force(1);
  force.z = omni_force(2);
  pub_OmniForceFeedback.publish(force);
}
コード例 #12
0
    void Edge_V_V_GICPLandmark::linearizeOplus()
    {
      //  std::cout << "START Edge_V_V_GICPLandmark::linearizeOplus() " << std::endl;
      VertexPointXYZ* vp0 = static_cast<VertexPointXYZ*>(_vertices[0]);
      VertexSE3* vp1 = static_cast<VertexSE3*>(_vertices[1]);
      Vector3d p1 = measurement().pos1;

      if (!vp0->fixed())
        {
          _jacobianOplusXi.block<3,3>(0,0) = -Matrix3d::Identity();
        }

      if (!vp1->fixed())
        {
          Matrix3d R1 = vp1->estimate().matrix().topLeftCorner<3,3>();
          _jacobianOplusXj.block<3,3>(0,0) = R1;
          _jacobianOplusXj.block<3,1>(0,3) = R1*dRidx.transpose()*p1;
          _jacobianOplusXj.block<3,1>(0,4) = R1*dRidy.transpose()*p1;
          _jacobianOplusXj.block<3,1>(0,5) = R1*dRidz.transpose()*p1;
        }
    }
コード例 #13
0
void Camera::updateOthers() {
    P_ /= P_.row(2).head<3>().squaredNorm();

	// RQ Factorization
    Matrix3d M = P_.topLeftCorner<3, 3>();
	Matrix3d reverseRows;
	reverseRows << 0, 0, 1, 0, 1, 0, 1, 0, 0;

    const Eigen::Matrix3d qrMatrix = (reverseRows * M).transpose();
    const Eigen::HouseholderQR<Matrix3d> qrDecomp(qrMatrix);
    const Matrix3d matrixQ = qrDecomp.householderQ();
    const Matrix3d matrixR = qrDecomp.matrixQR().triangularView<Upper>();

	R_ = reverseRows * matrixQ.transpose();
	K_ = reverseRows * matrixR.transpose() * reverseRows;

	// Eigen doesn't require diagonal elements of R (in the QR
	// decomposition) to be positive, so correct for this since the
	// intrinsic matrix should have positive diagonal elements
	for(int axis = 2; axis >= 0; --axis) {
		if(K_(axis, axis) < 0) {
			K_(axis, axis) = -(K_(axis, axis));
			R_.row(axis) = -(R_.row(axis));
		}

		if(K_(axis, 2) < 0)
			K_(axis, 2) = -K_(axis, 2);
	}

	orthonormalize(R_);

	Kinv_ = K_.inverse();
	Rinv_ = R_.transpose();
	t_ = Kinv_ * P_.col(3);
	C_ = -Rinv_ * t_;

	updatePrincipleRay();
}
コード例 #14
0
ファイル: localizerGVG.cpp プロジェクト: Brenn10/gvg
void localizerGVG::propagate(double Vm, double Wm, double dt) {  
  Matrix3d Fr;
  Fr<<1, 0, -Vm*dt*sin(oldYaw),
      0, 1,  Vm*dt*cos(oldYaw),
      0, 0,  1;
  MatrixXd Gr(3,2);
  Gr<< -dt*cos(oldYaw), 0, 
       -dt*sin(oldYaw), 0,
        0,             -dt;
    
  P.block(0,0,3,3)=Fr*P.block(0,0,3,3)*Fr.transpose()+Gr*Qr*Gr.transpose();
  CF=Fr*CF;
  normalizeCovariance();
}
コード例 #15
0
ファイル: auxiliar.cpp プロジェクト: rubengooj/StVO-PL
Matrix3d skewlog(Matrix3d M){
    Matrix3d skew;
    double val = (M.trace() - 1.f)/2.f;
    if(val > 1.f)
        val = 1.f;
    else if (val < -1.f)
        val = -1.f;
    double theta = acos(val);
    if(theta == 0.f)
        skew << 0,0,0,0,0,0,0,0,0;
    else
        skew << (M-M.transpose())/(2.f*sin(theta))*theta;
    return skew;
}
コード例 #16
0
void QuaternionFloatingJoint::qdot2v(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
  qdot_to_v.resize(getNumVelocities(), getNumPositions());

  auto quat = q.middleRows<QUAT_SIZE>(SPACE_DIMENSION);
  Matrix3d R = quat2rotmat(quat);

  Vector4d quattilde;
  Matrix<double, SPACE_DIMENSION, QUAT_SIZE> M;
  Matrix<double, SPACE_DIMENSION, QUAT_SIZE> RTransposeM;
  Gradient<Vector4d, QUAT_SIZE, 1>::type dquattildedquat;
  if (dqdot_to_v) {
    Gradient<Vector4d, QUAT_SIZE, 2>::type ddquattildedquat;
    normalizeVec(quat, quattilde, &dquattildedquat, &ddquattildedquat);
    auto dR = dquat2rotmat(quat);
    Gradient<Matrix<double, SPACE_DIMENSION, QUAT_SIZE>, QUAT_SIZE, 1>::type dM;
    quatdot2angularvelMatrix(quat, M, &dM);

    RTransposeM.noalias() = R.transpose() * M;
    auto dRTranspose = transposeGrad(dR, R.rows());
    auto dRTransposeM = matGradMultMat(R.transpose(), M, dRTranspose, dM);
    auto dRTransposeMdquattildedquat = matGradMultMat(RTransposeM, dquattildedquat, dRTransposeM, ddquattildedquat);
    dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
    setSubMatrixGradient<4>(*dqdot_to_v, dRTranspose, intRange<3>(3), intRange<3>(0), qdot_to_v.rows(), 3);
    setSubMatrixGradient<4>(*dqdot_to_v, dRTransposeMdquattildedquat, intRange<3>(0), intRange<4>(3), qdot_to_v.rows(), 3);
  }
  else {
    normalizeVec(quat, quattilde, &dquattildedquat);
    quatdot2angularvelMatrix(quat, M);
    RTransposeM.noalias() = R.transpose() * M;
  }
  qdot_to_v.block<3, 3>(0, 0).setZero();
  qdot_to_v.block<3, 4>(0, 3).noalias() = RTransposeM * dquattildedquat;
  qdot_to_v.block<3, 3>(3, 0) = R.transpose();
  qdot_to_v.block<3, 4>(3, 3).setZero();
}
コード例 #17
0
ファイル: State.cpp プロジェクト: bchretien/dart
//==============================================================================
double State::getCoronalRightLegAngle() const
{
  Matrix3d comR = getCOMFrame().linear();
  Vector3d comY = comR.col(1);
  Vector3d thighAxisZ = mRightThigh->getTransform().linear().col(2);
  Vector3d projThighAZ = (comR.transpose() * thighAxisZ);
  projThighAZ[0] = 0.0;
  projThighAZ.normalize();
  double angle = _getAngleBetweenTwoVectors(projThighAZ, comY);

  Vector3d cross = comY.cross(projThighAZ);

  if (cross[0] > 0.0)
    return angle;
  else
    return -angle;
}
コード例 #18
0
ファイル: State.cpp プロジェクト: bchretien/dart
//==============================================================================
double State::getCoronalPelvisAngle() const
{
  Matrix3d comR = getCOMFrame().linear();
  Vector3d comY = comR.col(1);
  Vector3d pelvisZ = mPelvis->getTransform().linear().col(2);
  Vector3d projPelvisZ = (comR.transpose() * pelvisZ);
  projPelvisZ[0] = 0.0;
  projPelvisZ.normalize();
  double angle = _getAngleBetweenTwoVectors(projPelvisZ, comY);

  Vector3d cross = comY.cross(projPelvisZ);

  if (cross[0] > 0.0)
    return angle;
  else
    return -angle;
}
コード例 #19
0
Matrix<double,Dynamic,9>
PatchFit::general_dfda (MatrixXd ql, Matrix3d rr, MatrixXd dfdql, vector<MatrixXd> dqldr)
{
  MatrixXd dfdk (ql.rows(), ql.cols());
  dfdk = ql.array()*ql.array();

  MatrixXd dfdr (ql.rows(), ql.cols());
  dfdr.col(0) = (dfdql.array()*dqldr[0].array()).rowwise().sum();
  dfdr.col(1) = (dfdql.array()*dqldr[1].array()).rowwise().sum();
  dfdr.col(2) = (dfdql.array()*dqldr[2].array()).rowwise().sum();

  MatrixXd dfdc (ql.rows(), ql.cols());
  dfdc = -dfdql*rr.transpose();

  MatrixXd j (ql.rows(), 9);
  j << dfdk, dfdr, dfdc;

  return j;
}
コード例 #20
0
JNIEXPORT jobject JNICALL Java_com_mousebird_maply_Matrix3d_transpose
(JNIEnv *env, jobject obj)
{
    try
    {
        Matrix3dClassInfo *classInfo = Matrix3dClassInfo::getClassInfo();
        Matrix3d *inst = classInfo->getObject(env,obj);
        if (!inst)
            return NULL;
        
        Matrix3d matTrans = inst->transpose();
        return MakeMatrix3d(env,matTrans);
    }
    catch (...)
    {
        __android_log_print(ANDROID_LOG_VERBOSE, "Maply", "Crash in Matrix3d::transpose()");
    }
    
    return NULL;
}
コード例 #21
0
ファイル: main.cpp プロジェクト: cjld/MeshDeform
void ckAlign() {
    srand(time(0));
    Matrix3d t = Matrix3d::Random();
    Matrix3d r,s;
    polarDec(t,r,s);
    vector<Vector3d> v1, v2;
    for (int i=0; i<2; i++) {
        Vector3d v(Vector3d::Random());
        v1.push_back(v);
        //v2.push_back(r*v);
        v2.push_back(Vector3d::Random());
    }
    RotateAlign align(v1, v2);
    Matrix3d rr = align.calc();

    cout << rr << endl;
    cout << r << endl;
    cout << (r-rr.transpose()).norm() << endl;

    align.ckt(rr);
}
コード例 #22
0
ファイル: auxiliar.cpp プロジェクト: rubengooj/StVO-PL
Vector3d logarithm_map_so3(Matrix3d R){
    Matrix3d Id3 = Matrix3d::Identity();
    Vector3d w;
    Matrix3d V = Matrix3d::Identity(), w_hat = Matrix3d::Zero();
    w << 0.f, 0.f, 0.f;
    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);
    }
    return w;
}
コード例 #23
0
ファイル: frame.cpp プロジェクト: RoboWGT/robo_groovy
  // translate kpts by 6DOF transformation of frame
  void Frame::setTKpts(Eigen::Vector4d trans, Eigen::Quaterniond rot)
  {
    Vector3d tr;
    tr = trans.head<3>();
    // set up 3x4 transformation from kpt+disp to kpt
    Matrix4d Q;
    Q << 1.0, 0.0, 0.0, -cam.cx, // fy should enter in here somewhere...
         0.0, 1.0, 0.0, -cam.cy,
         0.0, 0.0, 0.0, cam.fx,
         0.0, 0.0, 1.0/cam.tx, 0;
    Matrix<double,3,4> P;
    P << cam.fx, 0.0, cam.cx, 0.0,
         0.0, cam.fx, cam.cy, 0.0,
         0.0, 0.0, 1.0, 0.0;
    // 3D point transform - inverse of frame motion
    Matrix4d T;
    T.setZero();
    Matrix3d R = rot.toRotationMatrix();
    T.block<3,3>(0,0) = R.transpose();
    T.block<3,1>(0,3) = -R*tr;
    T(3,3) = 1.0;
    
    P = P*T*Q;

    // go through points and set up transformed ones
    tkpts.resize(kpts.size());
    Vector4d v(0.0,0.0,0.0,1.0);
    for (int i=0; i<(int)kpts.size(); i++)
      {
        Vector3d vk;
        v(0) = kpts[i].pt.x;
        v(1) = kpts[i].pt.y;
        v(2) = disps[i];
        vk = P*v;
        tkpts[i].pt.x = vk(0)/vk(2);
        tkpts[i].pt.y = vk(1)/vk(2);
      }
  }
コード例 #24
0
Vector3d
SpiceRotationModel::angularVelocity(double tdbSec) const
{
    double et = tdbSec;
    SpiceDouble transform[6][6];

    sxform_c(m_fromFrame.c_str(), m_toFrame.c_str(), et, transform);
    if (!failed_c())
    {
        // Extract the angular velocity vector from the state transform matrix
        // Rotation matrix is the top left 3x3 matrix
        Matrix3d R;
        R << transform[0][0], transform[0][1], transform[0][2],
             transform[1][0], transform[1][1], transform[1][2],
             transform[2][0], transform[2][1], transform[2][2];

        // W*R is the lower left 3x3 matrix
        Matrix3d WR;
        WR << transform[3][0], transform[3][1], transform[3][2],
              transform[4][0], transform[4][1], transform[4][2],
              transform[5][0], transform[5][1], transform[5][2];

        // Multiply by inverse of R (= transpose, since R is a rotation) to get
        // the skew-symmetric matrix W*
        Matrix3d W = WR * R.transpose();

        return Vector3d(-W(1, 2), W(0, 2), -W(0, 1));
    }
    else
    {
        char errorMessage[1024];
        getmsg_c("long", sizeof(errorMessage), errorMessage);
        cerr << errorMessage << std::endl;
        reset_c();
        return Vector3d::Zero();
    }
}
コード例 #25
0
ファイル: conics.cpp プロジェクト: stevenlovegrove/fiducials
Conic UnmapConic( const Conic& c, const AbstractCamera& cam )
{
  std::vector<Eigen::Vector2d > d;
  std::vector<Eigen::Vector2d > u;

  d.push_back(c.center);
  d.push_back(Eigen::Vector2d(c.bbox.x1,c.bbox.y1));
  d.push_back(Eigen::Vector2d(c.bbox.x1,c.bbox.y2));
  d.push_back(Eigen::Vector2d(c.bbox.x2,c.bbox.y1));
  d.push_back(Eigen::Vector2d(c.bbox.x2,c.bbox.y2));

  for( int i=0; i<5; ++i )
    u.push_back( cam.unmap(d[i]) );

  // Distortion locally estimated by homography
  const Matrix3d H_du = EstimateH_ba(u,d);

  Conic ret;
//  ret.bbox = c.bbox;
  ret.C = H_du.transpose() * c.C * H_du;
  ret.Dual = ret.C.inverse();
  ret.center = u[0];
  return ret;
}
コード例 #26
0
            bodyId_, controlPoint_, true);
        tare = 0;
    }
  
    // Get the latest joint state information
    Vector Q(model.getNumDOFs());
    Vector Qd(model.getNumDOFs());
  
    model.getLatestFullState(Q, Qd);
  
    Vector3d bodyTranslation = RigidBodyDynamics::CalcBodyToBaseCoordinates(model.rbdlModel(), Q, bodyId_, Vector::Zero(3), false);
    Matrix3d bodyRotation =  RigidBodyDynamics::CalcBodyWorldOrientation(model.rbdlModel(), Q, bodyId_, false);
  
    getJacobian(JtLoc);
  
    xCurWorld = bodyRotation.transpose() * controlPoint_ + bodyTranslation;
    Matrix3d frameRotation;
    Vector3d frameTranslation;

    if(frameId_ == -1) //Using fixed world frame as reference, latching immaterial.
    {

        // if (projection_.cols() != xCurWorld.rows())
        // {
        //     CONTROLIT_ERROR << "Matrix size problem:\n"
        //                     << "  - frameId_ = " << frameId_ << "\n"
        //                     << "  - projection_ = " << projection_ << "\n"
        //                     << "  - xCurWorld_ = " << xCurWorld;
        // }

        xCurFrameProjected = projection_ * xCurWorld;
コード例 #27
0
ファイル: ekf_node.cpp プロジェクト: MorS25/switch_vis_exp
 // Gets camera velocities. Also, if target not visible (i.e. estimatorOn = false) publishes output (ground truth)
 // and does prediction 
 void camVelCB(const geometry_msgs::TwistStampedConstPtr& twist)
 {
     // Time
     ros::Time timeStamp = twist->header.stamp;
     double timeNow = timeStamp.toSec();
     double delT = timeNow - lastVelTime;
     lastVelTime = timeNow;
     
     // Camera velocities, expressed in camera coordinate system
     vCc << twist->twist.linear.x,twist->twist.linear.y,twist->twist.linear.z;
     wGCc << twist->twist.angular.x,twist->twist.angular.y,twist->twist.angular.z;
     
     if (!estimatorOn)
     {
         // Object trans w.r.t. image frame, for ground truth
         Vector3d trans;
         tf::StampedTransform transform;
         tfl.waitForTransform("image","ugv0",timeStamp,ros::Duration(0.1));
         tfl.lookupTransform("image","ugv0",timeStamp,transform);
         tf::Vector3 temp_trans = transform.getOrigin();
         trans << temp_trans.getX(),temp_trans.getY(),temp_trans.getZ();
         
         // Ground truth
         Vector3d x;
         x << trans.segment<2>(0)/trans(2),1/trans(2);
         xlast << x; // Update for optical flow
         
         // Object rotation w.r.t. image frame, for rotating target velocities into image coordinates
         try
         {
             Quaterniond quat;
             if (deadReckoning)
             {
                 tf::StampedTransform tfImage2World;
                 tf::StampedTransform tfOdom2Marker;
                 tfl.waitForTransform("image","world",timeStamp,ros::Duration(0.1));
                 tfl.lookupTransform("image","world",timeStamp,tfImage2World);
                 tfl.waitForTransform("ugv0/odom","ugv0/base_footprint",timeStamp,ros::Duration(0.1));
                 tfl.lookupTransform("ugv0/odom","ugv0/base_footprint",timeStamp,tfOdom2Marker);
                 
                 tf::Quaternion temp_quat = tfImage2World.getRotation();
                 Quaterniond qIm2W = Quaterniond(temp_quat.getW(),temp_quat.getX(),temp_quat.getY(),temp_quat.getZ());
                 temp_quat = tfOdom2Marker.getRotation();
                 Quaterniond qO2M = Quaterniond(temp_quat.getW(),temp_quat.getX(),temp_quat.getY(),temp_quat.getZ());
                 quat = qIm2W*qWorld2Odom*qO2M;
             }
             else
             {
                 tf::Quaternion temp_quat = transform.getRotation();
                 quat = Quaterniond(temp_quat.getW(),temp_quat.getX(),temp_quat.getY(),temp_quat.getZ());
             }
             
             // Target velocities expressed in camera coordinates
             Vector3d vTc = quat*vTt;
             
             // Update so that delT in featureCB is reasonable after switch
             lastImageTime = timeNow;
             
             // Convert to scalars to match notation in papers
             double vc1 = vCc(0);        double vc2 = vCc(1);        double vc3 = vCc(2);
             double vq1 = vTc(0);        double vq2 = vTc(1);        double vq3 = vTc(2);
             double w1 = wGCc(0);        double w2 = wGCc(1);        double w3 = wGCc(2);
             double x1hat = xhat(0);     double x2hat = xhat(1);     double x3hat = xhat(2);
             
             // Predictor
             double Omega1 = w3*x2hat - w2 - w2*pow(x1hat,2) + w1*x1hat*x2hat;
             double Omega2 = w1 - w3*x1hat - w2*x1hat*x2hat + w1*pow(x2hat,2);
             double xi1 = (vc3*x1hat - vc1)*x3hat;
             double xi2 = (vc3*x2hat - vc2)*x3hat;
             
             double x1hatDot = Omega1 + xi1 + vq1*x3hat - x1hat*vq3*x3hat;
             double x2hatDot = Omega2 + xi2 + vq2*x3hat - x2hat*vq3*x3hat;
             double x3hatDot = vc3*pow(x3hat,2) - (w2*x1hat - w1*x2hat)*x3hat - vq3*pow(x3hat,2);
             
             // Predict Covariance
             Matrix3d F = calculate_F(xhat,vCc,vTc,wGCc);
             MatrixXd Pdot = F*P+P*F.transpose() + Q;
             
             // Update states and covariance
             Vector3d xhatDot;
             xhatDot << x1hatDot, x2hatDot, x3hatDot;
             xhat += xhatDot*delT;
             P += Pdot*delT;
             
             // Publish output
             publishOutput(x,xhat,trans,timeStamp);
         }
         catch (tf::TransformException e)
         {
         }
     }
 }
コード例 #28
0
void
basic_ins_qkf::predict_ned(const Vector3d& gyro_meas,
		const Vector3d& accel_meas,
		double dt)
{
#ifdef TIME_OPS
	timer clock;
	clock.start();
#endif
	Vector3d accel_body = avg_state.orientation.conjugate()*accel_meas;
	// Vector3d accel_dir = accel_body.normalized();
	Matrix<double, 3, 3> accel_cov = cross<double>(-accel_body);

	// 1500x realtime, without vectorization, on 2.2 GHz Athlon X2
	// See basic_ins_qkf::predict() for the full blockwise form
	const Matrix<double, 12, 12> pcov = cov;
	const Matrix3d dtR = dt * avg_state.orientation.conjugate().toRotationMatrix();
	const Matrix3d dtQ = accel_cov * dt;

	cov.block<3, 3>(0, 3) -= pcov.block<3,3>(0, 0)*dtR.transpose();
	cov.block<3, 3>(0, 6) += dt * pcov.block<3, 3>(0, 9);
	cov.block<3, 3>(0, 9) -= pcov.block<3, 3>(0, 3) * dtQ.transpose();
	cov.block<3, 3>(3, 3) += dtR*pcov.block<3, 3>(0, 0)*dtR.transpose()
			- dtR*pcov.block<3, 3>(0, 3) - pcov.block<3, 3>(3, 0)*dtR.transpose();
	cov.block<3, 3>(3, 6) += -dtR * (pcov.block<3, 3>(0, 6) + dt*pcov.block<3, 3>(0, 9))
			+ dt*pcov.block<3, 3>(3, 9);
	cov.block<3, 3>(3, 9) += -dtR*( -pcov.block<3, 3>(0, 3)*dtQ.transpose() + pcov.block<3, 3>(0, 9))
			- pcov.block<3, 3>(3, 3)*dtQ.transpose();
	cov.block<3, 3>(6, 6) += dt*pcov.block<3, 3>(6, 9) + dt*dt*pcov.block<3, 3>(9, 9)
			+ dt*pcov.block<3, 3>(9, 6);
	cov.block<3, 3>(6, 9) += -pcov.block<3, 3>(6, 3)*dtQ.transpose() + dt*pcov.block<3, 3>(9, 9)
			- dt*pcov.block<3, 3>(9, 3)*dtQ.transpose();
	cov.block<3, 3>(9, 9) += dtQ*pcov.block<3, 3>(3, 3)*dtQ.transpose()
			- dtQ*pcov.block<3, 3>(3, 9) - pcov.block<3, 3>(9, 3)*dtQ.transpose();

	// Update symmetric cross-covariance terms
	cov.block<3, 3>(3, 0) = cov.block<3, 3>(0, 3).transpose();
	cov.block<3, 3>(6, 0) = cov.block<3, 3>(0, 6).transpose();
	cov.block<3, 3>(6, 3) = cov.block<3, 3>(3, 6).transpose();
	cov.block<3, 3>(9, 0) = cov.block<3, 3>(0, 9).transpose();
	cov.block<3, 3>(9, 3) = cov.block<3, 3>(3, 9).transpose();
	cov.block<3, 3>(9, 6) = cov.block<3, 3>(6, 9).transpose();

	// Add state transition noise
	cov.block<3, 3>(0, 0) += gyro_stability_noise.asDiagonal() * dt;
	cov.block<3, 3>(3, 3) += gyro_white_noise.asDiagonal() * dt;
	cov.block<3, 3>(6, 6) += accel_white_noise.asDiagonal() * 0.5*dt*dt;
	cov.block<3, 3>(9, 9) += accel_white_noise.asDiagonal() * dt;

	Quaterniond orientation = exp<double>((gyro_meas - avg_state.gyro_bias) * dt)
			* avg_state.orientation;
	// The contribution of the acceleration due to gravity on the accelerometer.
	// Note that in NED, the z axis is down, and the accelerometer observes
	// an extra acceleration due to gravity in the "up" direction.
	Vector3d accel_gravity = -Vector3d::UnitZ() * 9.81;
	Vector3d accel = accel_body - accel_gravity;
	Vector3d position = avg_state.position + avg_state.velocity * dt + 0.5*accel*dt*dt;
	Vector3d velocity = avg_state.velocity + accel*dt;

	avg_state.position = position;
	avg_state.velocity = velocity;
	// Note: Renormalization occurs during all measurement updates.
	avg_state.orientation = orientation;

	assert(invariants_met());
#ifdef TIME_OPS
	double time = clock.stop()*1e6;
	std::cout << "unscented predict time: " << time << "\n";
#endif
}
コード例 #29
0
ファイル: homography.cpp プロジェクト: zangel/uquad
bool Homography::
decompose()
{
  decompositions.clear();
  JacobiSVD<MatrixXd> svd(H_c2_from_c1, ComputeThinU | ComputeThinV);

  Vector3d singular_values = svd.singularValues();

  double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT)
  double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book.
  double d3 = fabs(singular_values[2]);

  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV();                    // VT^T

  double s = U.determinant() * V.determinant();

  double dPrime_PM = d2;

  int nCase;
  if(d1 != d2 && d2 != d3)
    nCase = 1;
  else if( d1 == d2 && d2 == d3)
    nCase = 3;
  else
    nCase = 2;

  if(nCase != 1)
  {
    printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. ");
    return false;
  }

  double x1_PM;
  double x2;
  double x3_PM;

  // All below deals with the case = 1 case.
  // Case 1 implies (d1 != d3)
  { // Eq. 12
    x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3));
    x2    = 0;
    x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3));
  };

  double e1[4] = {1.0,-1.0, 1.0,-1.0};
  double e3[4] = {1.0, 1.0,-1.0,-1.0};

  Vector3d np;
  HomographyDecomposition decomp;

  // Case 1, d' > 0:
  decomp.d = s * dPrime_PM;
  for(size_t signs=0; signs<4; signs++)
  {
    // Eq 13
    decomp.R = Matrix3d::Identity();
    double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
    double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2;
    decomp.R(0,0) = dCosTheta;
    decomp.R(0,2) = -dSinTheta;
    decomp.R(2,0) = dSinTheta;
    decomp.R(2,2) = dCosTheta;

    // Eq 14
    decomp.t[0] = (d1 - d3) * x1_PM * e1[signs];
    decomp.t[1] = 0.0;
    decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs];

    np[0] = x1_PM * e1[signs];
    np[1] = x2;
    np[2] = x3_PM * e3[signs];
    decomp.n = V * np;

    decompositions.push_back(decomp);
  }

  // Case 1, d' < 0:
  decomp.d = s * -dPrime_PM;
  for(size_t signs=0; signs<4; signs++)
  {
    // Eq 15
    decomp.R = -1 * Matrix3d::Identity();
    double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
    double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;
    decomp.R(0,0) = dCosPhi;
    decomp.R(0,2) = dSinPhi;
    decomp.R(2,0) = dSinPhi;
    decomp.R(2,2) = -dCosPhi;

    // Eq 16
    decomp.t[0] = (d1 + d3) * x1_PM * e1[signs];
    decomp.t[1] = 0.0;
    decomp.t[2] = (d1 + d3) * x3_PM * e3[signs];

    np[0] = x1_PM * e1[signs];
    np[1] = x2;
    np[2] = x3_PM * e3[signs];
    decomp.n = V * np;

    decompositions.push_back(decomp);
  }

  // Save rotation and translation of the decomposition
  for(unsigned int i=0; i<decompositions.size(); i++)
  {
    Matrix3d R = s * U * decompositions[i].R * V.transpose();
    Vector3d t = U * decompositions[i].t;
    decompositions[i].T = Sophus::SE3(R, t);
  }
  return true;
}
コード例 #30
0
void DecomposeEssentialUsingHorn90(double _E[9], double _R1[9], double _R2[9], double _t1[3], double _t2[3]) {
	//from : http://people.csail.mit.edu/bkph/articles/Essential.pdf
#ifdef USE_EIGEN
	using namespace Eigen;

	Matrix3d E = Map<Matrix<double,3,3,RowMajor> >(_E);
	Matrix3d EEt = E * E.transpose();
	Vector3d e0e1 = E.col(0).cross(E.col(1)),e1e2 = E.col(1).cross(E.col(2)),e2e0 = E.col(2).cross(E.col(0));
	Vector3d b1,b2;

#if 1
	//Method 1
	Matrix3d bbt = 0.5 * EEt.trace() * Matrix3d::Identity() - EEt; //Horn90 (12)
	Vector3d bbt_diag = bbt.diagonal();
	if (bbt_diag(0) > bbt_diag(1) && bbt_diag(0) > bbt_diag(2)) {
		b1 = bbt.row(0) / sqrt(bbt_diag(0));
		b2 = -b1;
	} else if (bbt_diag(1) > bbt_diag(0) && bbt_diag(1) > bbt_diag(2)) {
		b1 = bbt.row(1) / sqrt(bbt_diag(1));
		b2 = -b1;
	} else {
		b1 = bbt.row(2) / sqrt(bbt_diag(2));
		b2 = -b1;
	}
#else
	//Method 2
	if (e0e1.norm() > e1e2.norm() && e0e1.norm() > e2e0.norm()) {
		b1 = e0e1.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	} else if (e1e2.norm() > e0e1.norm() && e1e2.norm() > e2e0.norm()) {
		b1 = e1e2.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	} else {
		b1 = e2e0.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	}
#endif
	
	//Horn90 (19)
	Matrix3d cofactors; cofactors.col(0) = e1e2; cofactors.col(1) = e2e0; cofactors.col(2) = e0e1;
	cofactors.transposeInPlace();
	
	//B = [b]_x , see Horn90 (6) and http://en.wikipedia.org/wiki/Cross_product#Conversion_to_matrix_multiplication
	Matrix3d B1; B1 <<	0,-b1(2),b1(1),
						b1(2),0,-b1(0),
						-b1(1),b1(0),0;
	Matrix3d B2; B2 <<	0,-b2(2),b2(1),
						b2(2),0,-b2(0),
						-b2(1),b2(0),0;

	Map<Matrix<double,3,3,RowMajor> > R1(_R1),R2(_R2);

	//Horn90 (24)
	R1 = (cofactors.transpose() - B1*E) / b1.dot(b1);
	R2 = (cofactors.transpose() - B2*E) / b2.dot(b2);
	Map<Vector3d> t1(_t1),t2(_t2); 
	t1 = b1; t2 = b2;
	
	cout << "Horn90 provided " << endl << R1 << endl << "and" << endl << R2 << endl;
#endif
}