コード例 #1
0
ファイル: 02-navigate.cpp プロジェクト: cerdogan/iserFinal
/// Determines the goal configuration of the left arm based on the cinder block pose, calls
/// analytical I.K. to find a suitable goal arm configuration and calls RRT routine to get the
/// appropriate trajectory. Finally, using the Timer routine, displays the trajectory.
void SimTab::OnButton(wxCommandEvent &evt) {

	static int i = 0;
	i = (i + 1) % 4;

	// Set the initial config if restarting
	if(i == 3) {
		krang->setConfig(planar_ids, Eigen::Vector3d(0.0, 0.0, -M_PI_2));
		viewer->DrawGLScene();
		return;
	}

	// Get the normal and the location of the cinder block
	Eigen::Matrix4d cinderT = mWorld->getSkeleton("Cinder2")->getNode("root")->getWorldTransform();
	Eigen::Vector3d normal = -cinderT.block<3,1>(0,2);
	Eigen::Vector3d hole (cinderT(0,3), cinderT(1,3), 0.44);
	hole += normal * 0.0945;
	cout << "\nhole: " << hole.transpose() << endl;
	cout << "normal: " << normal.transpose() << endl;

	// Determine where we want Krang to be
	Eigen::Vector3d perp (-normal(1), normal(0), 0.0);
	Eigen::Vector3d goalPos = hole + normal * 0.3 + perp * 0.6;
	cout << "goalPos: " << goalPos.transpose() << endl;
	double th = atan2(normal(0), -normal(1)) + 0.1;
	Eigen::Vector3d goalConfig (goalPos(0), goalPos(1), th);

	// Determine the angle we need to be to go the goal position
	double th2 = atan2(-goalPos(0), goalPos(1));
	//cout << th2 << " " << sqrt(goalPos(0) * goalPos(0) + goalPos(1) * goalPos(1)) << " " << th << endl;


	static const double wheelRadius = 0.264;	// r, cm
	static const double rotationRadius = 0.350837; // R, cm
	double v1 = (th2 * rotationRadius) / wheelRadius;
	double v3 = (th * rotationRadius) / wheelRadius;
	double dist = sqrt(goalPos(0) * goalPos(0) + goalPos(1) * goalPos(1));
	double v2 = dist / wheelRadius;
	Eigen::Vector3d params (v1,v2,v3);
	cout << "params: " << params.transpose() << endl;

	if(i == 2) {
		cout << "th: " << th << endl;
		krang->setConfig(planar_ids, Eigen::Vector3d(goalPos(0), goalPos(1), th));
		viewer->DrawGLScene();
		return;
	}
	if(i == 0) {
		cout << "th2: " << th2 << endl;
		krang->setConfig(planar_ids, Eigen::Vector3d(0.0, 0.0, th2));
		viewer->DrawGLScene();
		return;
	}

	// Draw the robot at the goal position having just arrived
	if(i == 1) {
		krang->setConfig(planar_ids, Eigen::Vector3d(goalPos(0), goalPos(1), th2));
		viewer->DrawGLScene();
	}
}
コード例 #2
0
/*
 * Given the line parameters  [n_x,n_y,a_x,a_y] check if
 * [n_x, n_y] dot [data.x-a_x, data.y-a_y] < m_delta
 */
bool RSTEstimator::agree(std::vector<double> &parameters, std::pair<Eigen::Vector3d,Eigen::Vector3d> &data)
{
	
	Eigen::Matrix3d R;
	Eigen::Vector3d T;
	Eigen::Vector3d dif;
	double  E1,E2;
	
	
	R << parameters[0] , parameters[1] , parameters[2],
	     parameters[3] , parameters[4] , parameters[5],
	     parameters[6] , parameters[7] , parameters[8];
	
	T << parameters[9] , parameters[10] , parameters[11];
	

    dif = data.first - R*data.second + T; //X21
	E1 = dif.transpose()*dif;
	
	dif = data.second - R.inverse() * (data.first-T); //X12
	E2 = dif.transpose()*dif;
	
	//std::cout << "E1= " << E1 << "\nE2= " << E2 << "\nE1+E2= "<< E1+E2 << " " << this->deltaSquared <<"\n";
	
	return ( (E1 + E2) < this->deltaSquared);
}
コード例 #3
0
ファイル: gicp.hpp プロジェクト: 87west/pcl
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);
}
コード例 #4
0
int main(int argc, char* argv[])
{
  Eigen::Vector3d geoInit(16,42,1000);

  Eigen::Vector3d xyz = labust::tools::geodetic2ecef(geoInit);
  std::cout<<"Origin, GEO->ECEF:"<<xyz.transpose()<<std::endl;
  Eigen::Vector3d ned = labust::tools::ecef2ned(xyz, geoInit);
  std::cout<<"Origin, ECEF->NED:"<<ned.transpose()<<std::endl;
  Eigen::Vector3d xyz2 = labust::tools::ned2ecef(ned, geoInit);
  std::cout<<"Origin, NED->ECEF:"<<xyz2.transpose()<<std::endl;
  Eigen::Vector3d geoInit2 = labust::tools::ecef2geodetic(xyz2);
  std::cout<<"Origin, ECEF->GEO:"<<geoInit2.transpose()<<std::endl;

  std::cout<<std::endl;
  Eigen::Vector3d nedR(1000, 5000, 1000);
  std::cout<<"Random NED point: n="<<nedR(0)<<", e="<<nedR(1)<<", d="<<nedR(2)<<std::endl;

  Eigen::Vector3d xyz3 = labust::tools::ned2ecef(nedR, geoInit);
  std::cout<<"Random, NED->ECEF:"<<xyz3.transpose()<<std::endl;
  Eigen::Vector3d geo3 = labust::tools::ecef2geodetic(xyz3);
  std::cout<<"Random, ECEF->GEO:"<<geo3.transpose()<<std::endl;
  Eigen::Vector3d xyz4 = labust::tools::geodetic2ecef(geo3);
  std::cout<<"Random, GEO->ECEF:"<<xyz4.transpose()<<std::endl;
  Eigen::Vector3d ned4 = labust::tools::ecef2ned(xyz4, geoInit);
  std::cout<<"Random, ECEF->NED:"<<ned4.transpose()<<std::endl;

	return 0;
}
コード例 #5
0
void CylindricalNVecConstr::impl_jacobian(jacobian_t& jac, const argument_t& x) const
{
  jac.reserve(jac_.dof());

  pgdata_->x(x);

  sva::PTransformd pos = surfaceFrame_*pgdata_->mbc().bodyPosW[bodyIndex_];
  Eigen::Vector3d vec = (targetFrame_.translation() - pos.translation());
  double dot = vec.dot(pos.rotation().row(2));
  double sign = std::copysign(1., dot);
  double vecNDot = sign*2.*vec.dot(pos.rotation().row(2));

  const Eigen::MatrixXd& jacVecNMat =
      jac_.vectorJacobian(pgdata_->mb(), pgdata_->mbc(),
                          surfaceFrame_.rotation().row(2).transpose());

  jacMat_.row(0).noalias() = vecNDot*vec.transpose()*jacVecNMat.block(3, 0, 3, jac_.dof());

  jac_.point(surfaceFrame_.translation());
  const Eigen::MatrixXd& jacMat = jac_.jacobian(pgdata_->mb(), pgdata_->mbc());
  jac_.point(Eigen::Vector3d::Zero());

  jacMat_.row(0).noalias() -= vecNDot*pos.rotation().row(2)*jacMat.block(3, 0, 3, jac_.dof());
  jacMat_.row(0).noalias() += (2.*vec.transpose())*jacMat.block(3, 0, 3, jac_.dof());

  fullJacobianSparse(pgdata_->mb(), jac_, jacMat_,
                     jac, {0, pgdata_->qParamsBegin()});
}
コード例 #6
0
ファイル: 05-standUp.cpp プロジェクト: cerdogan/iserFinal
/// Get the joint values from the encoders and the imu and compute the center of mass as well 
void getState(Vector6d& state, double dt) {

	// Read motor encoders, imu and ft and update dart skeleton
  hw->updateSensors(dt);

	// Calculate the COM of the body
	Eigen::Vector3d comRobot = krang->getWorldCOM();
	comRobot(2) -= 0.264;
	comRobot(0) += 0.00;  
	if(dbg) cout << "comRobot: " << comRobot.transpose() << endl;

	// Get the com of the object
	Eigen::Matrix4d T = krang->getNode("lGripper")->getWorldTransform();
	Eigen::Vector3d objectCOM = T.topRightCorner<3,1>() + T.block<3,1>(0,2) * 0.20;
	if(dbg) cout << "R: " << T.topLeftCorner<3,3>() << endl;

	// Combine the two coms
	Eigen::Vector3d com = (comRobot * 145.0 + objectCOM * 13.2) / (145 + 13.2);
	if(dbg) cout << "com: " << com.transpose() << endl;

	// Update the state (note for amc we are reversing the effect of the motion of the upper body)
	state(0) = atan2(com(0), com(2));
	state(1) = hw->imuSpeed;
	state(2) = (hw->amc->pos[0] + hw->amc->pos[1]) / 2.0 + hw->imu;
	state(3) = (hw->amc->vel[0] + hw->amc->vel[1]) / 2.0 + hw->imuSpeed;
	state(4) = (hw->amc->pos[1] - hw->amc->pos[0]) / 2.0;
	state(5) = (hw->amc->vel[1] - hw->amc->vel[0]) / 2.0;
}
コード例 #7
0
void RectangleHandler::initRectangle(const Eigen::VectorXd& Sw, double lambda,
		const Eigen::VectorXd& z, Eigen::VectorXd& shapeParams,
		Eigen::VectorXd& F) {

	//Get the points
	Eigen::Vector3d m1(z[0], z[1], 1);
	Eigen::Vector3d m2(z[2], z[3], 1);
	Eigen::Vector3d m3(z[4], z[5], 1);
	Eigen::Vector3d m4(z[6], z[7], 1);

	Eigen::Vector3d Ct(Sw[0], Sw[1], Sw[2]);
	Eigen::Quaterniond Cq(Sw[3], Sw[4], Sw[5], Sw[6]);

	//compute normals
	double c2 = (m1.cross(m3).transpose() * m4)[0]
			/ (m2.cross(m3).transpose() * m4)[0];
	double c3 = (m1.cross(m3).transpose() * m2)[0]
			/ (m4.cross(m3).transpose() * m2)[0];

	Eigen::Vector3d n2 = c2 * m2 - m1;
	Eigen::Vector3d n3 = c3 * m4 - m1;

	//Compute rotation matrix columns
	Eigen::Vector3d R1 = _K.inverse() * n2;
	R1 = R1 / R1.norm();

	Eigen::Vector3d R2 = _K.inverse() * n3;
	R2 = R2 / R2.norm();

	Eigen::Vector3d R3 = R1.cross(R2);

	//Compute frame quaternion
	Eigen::Matrix3d R;
	R << R1, R2, R3;

	const Eigen::Quaterniond qOC(R);

	Eigen::Quaterniond Fqhat = Cq * qOC;

	//Compute frame transaltion
	Eigen::Matrix3d omega = _K.transpose().inverse() * _K.inverse();
	double ff = sqrt(
			(n2.transpose() * omega * n2)[0] / (n3.transpose() * omega * n3)[0]);

	Eigen::Vector3d CtOhat = lambda * _K.inverse() * m1;
	Eigen::Vector3d Fthat = Ct + Cq.toRotationMatrix() * CtOhat;

	//compute shape parameters
	Eigen::Vector3d X = _K * R1;
	Eigen::Vector3d Y = c2 * lambda * m2 - lambda * m1;

	double w = ((X.transpose() * X).inverse() * X.transpose() * Y)[0];
	double h = w / ff;

	//Write the results
	shapeParams << w, h;
	F << Fthat[0], Fthat[1], Fthat[2], Fqhat.w(), Fqhat.x(), Fqhat.y(), Fqhat.z();

}
コード例 #8
0
void AnchoredRectangleHandler::initRectangle(const Eigen::VectorXd& Fw,
    double lambda, const Eigen::VectorXd& z, Eigen::VectorXd& shapeParamshat,
    Eigen::VectorXd& FOhphat, Eigen::VectorXd &FOqhat) {

  //Get the points
  Eigen::Vector3d m1(z[0], z[1], 1);
  Eigen::Vector3d m2(z[2], z[3], 1);
  Eigen::Vector3d m3(z[4], z[5], 1);
  Eigen::Vector3d m4(z[6], z[7], 1);

  Eigen::Vector3d Ft(Fw[0], Fw[1], Fw[2]);
  Eigen::Quaterniond Fq(Fw[3], Fw[4], Fw[5], Fw[6]);

  //compute normals
  double c2 = (m1.cross(m3).transpose() * m4)[0]
      / (m2.cross(m3).transpose() * m4)[0];
  double c3 = (m1.cross(m3).transpose() * m2)[0]
      / (m4.cross(m3).transpose() * m2)[0];

  Eigen::Vector3d n2 = c2 * m2 - m1;
  Eigen::Vector3d n3 = c3 * m4 - m1;

  //Compute rotation matrix columns
  Eigen::Vector3d R1 = _K.inverse() * n2;
  R1 = R1 / R1.norm();

  Eigen::Vector3d R2 = _K.inverse() * n3;
  R2 = R2 / R2.norm();

  Eigen::Vector3d R3 = R1.cross(R2);

  //Compute rotation from camera to object
  Eigen::Matrix3d R;
  R << R1, R2, R3;
  Eigen::Quaterniond FOq_e(R);

  // and initialize the of the object with respect to the anchor frame
  FOqhat << FOq_e.w(), FOq_e.x(), FOq_e.y(), FOq_e.z();

  // now initialize lower left corner homogeneous point
  FOhphat << z[0], z[1], 1.0;
  FOhphat = _K.inverse() * FOhphat;
  FOhphat(2) = 1.0 / lambda; // 1/d distance of the plane parallel to the image plane on which features are initialized.

  //Compute frame transaltion
  Eigen::Matrix3d omega = _K.transpose().inverse() * _K.inverse();
  double ff = sqrt(
      (n2.transpose() * omega * n2)[0] / (n3.transpose() * omega * n3)[0]);

  //compute shape parameters
  Eigen::Vector3d X = _K * R1;
  Eigen::Vector3d Y = c2 * lambda * m2 - lambda * m1;

  double w = ((X.transpose() * X).inverse() * X.transpose() * Y)[0];

  //Write the results
  shapeParamshat << ff, w / lambda;

}
コード例 #9
0
ファイル: drakeGeometryUtil.cpp プロジェクト: ElFeo/drake
Eigen::Matrix<double, 1, 11> dquatDiffAxisInvar(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2, const Eigen::Vector3d& u)
{
  Vector4d r = quatDiff(q1, q2);
  Matrix<double, 4, 8> dr = dquatDiff(q1, q2);
  Matrix<double, 1, 11> de;
  const auto& rvec = r.tail<3>();
  de << 4.0 * r(0) * dr.row(0) + 4.0 * u.transpose() * rvec *u.transpose() * dr.block<3, 8>(1, 0), 4.0 * u.transpose() * rvec * rvec.transpose();
  return de;
}
コード例 #10
0
ファイル: maniptool.cpp プロジェクト: liqiang1980/VTFS
void ManipTool::update_nv(Eigen::Vector3d lv,Eigen::Vector3d& n_hat,Eigen::Vector3d& nv_dot_fb){
    P_bar = Eigen::Matrix3d::Identity()-n_hat*n_hat.transpose();
    nv_dot = -1*Gama_n*P_bar*L_n*n_hat;
    nv_dot_fb = nv_dot;
    L_n_dot = -beta_n*L_n+(1/(1+pow(lv.norm(),2)))*lv*lv.transpose();
    n_hat = n_hat+nv_dot;
    n_hat = n_hat/n_hat.norm();
    L_n = L_n + L_n_dot;

}
コード例 #11
0
ファイル: common.hpp プロジェクト: dengchengcheng/laser_slam
static double multiplyVectorsImplementation(Eigen::Vector3d a,
                                            Eigen::Vector3d b,
                                            gtsam::OptionalJacobian<1,3> Ha,
                                            gtsam::OptionalJacobian<1,3> Hb) {
  if(Ha)
    *Ha = b.transpose();

  if(Hb)
    *Hb = a.transpose();

  return a.transpose() * b;
}
コード例 #12
0
ファイル: interactions.cpp プロジェクト: nfrik/hardyplus
// NEED TO BE TESTED!!!!!
//
// A is alway an outside particle
//
//
double interactions::getLambdaBond(const Eigen::Vector3d A, Eigen::Vector3d B, double xmin, double xmax, double ymin, double ymax, double zmin, double zmax){

    if (abs(B(1)-A(1))<=DBLDBL_MIN) {
        //same y different x
        if (A(0)<xmin)
            return abs(B(0)-xmin)/abs(A(0)-B(0));
        else if (A(0)>xmax)
            return abs(B(0)-xmax)/abs(A(0)-B(0));
    }
    
    if (abs(B(0)-A(0))<=DBLDBL_MIN) {
        //same x different y
        if (A(1)<ymin)
            return abs(B(1)-ymin)/abs(A(1)-B(1));
        else if (A(1)>ymax)
            return abs(B(1)-ymax)/abs(A(1)-B(1));
    }
    
    double k=(B(1)-A(1))/(B(0)-A(0));
    double b=B(1)-k*B(0);
    
    double ym1=k*xmin+b;
    double xm1=xmin;
    double ym2=ymin;
    double xm2=(ymin-b)/k;
    double ym3=ymax;
    double xm3=(ymax-b)/k;
    double ym4=k*xmax+b;
    double xm4=xmax;
    int i,n;
    
    Eigen::MatrixXd M(4,3);
    M<<xm1,ym1,0,
       xm2,ym2,0,
       xm3,ym3,0,
       xm4,ym4,0;
    
    Eigen::Vector3d Mm(4);

    Mm(0)=((B.transpose()-M.row(0)).array().abs()+(A.transpose()-M.row(0)).array().abs()).sum();
    Mm(1)=((B.transpose()-M.row(1)).array().abs()+(A.transpose()-M.row(1)).array().abs()).sum();
    Mm(2)=((B.transpose()-M.row(2)).array().abs()+(A.transpose()-M.row(2)).array().abs()).sum();
    Mm(3)=((B.transpose()-M.row(3)).array().abs()+(A.transpose()-M.row(3)).array().abs()).sum();
    
    //Find minimal Mm
    for (n=i=0; i<4; i++)
        n=(Mm(i)<Mm(n))?i:n;
    
    return sqrt((B.transpose()-M.row(0)).array().square().sum())/sqrt((B-A).array().square().sum());
}
コード例 #13
0
bool IncrementalPlaneEstimator::
tryPoint(const Eigen::Vector3f& iPoint, const Eigen::Vector3f& iNormal,
         const float iMaxError, const float iMaxAngle) {
  const int n = mPoints.size();
  if (n < 2) return true;

  Eigen::Vector3d p = iPoint.cast<double>();
  Eigen::Vector3d sum = mSum+p;
  Eigen::Matrix3d sumSquared = mSumSquared + p*p.transpose();
  Eigen::Vector4f plane = getPlane(sum, sumSquared, n+1);
  if (std::abs(plane.head<3>().cast<float>().dot(iNormal)) <
      std::cos(iMaxAngle)) return false;

  std::vector<float> prevErrors2 = computeErrors(getCurrentPlane(), mPoints);
  float prevTotalError2 =
    std::accumulate(prevErrors2.begin(), prevErrors2.end(), 0.0f);

  std::vector<float> errors2 = computeErrors(plane, mPoints);
  errors2.push_back(computeError(plane, iPoint));
  float thresh2 = iMaxError*iMaxError;
  int numInliers = 0;
  float totalError2 = 0;
  for (float e2 : errors2) {
    totalError2 += e2;
    numInliers += (e2<=thresh2);
  }
  float deltaError2 = totalError2/(n+1) - prevTotalError2/n;
  return deltaError2 < thresh2/n;
  //return numInliers==(n+1);
}
コード例 #14
0
void IncrementalPlaneEstimator::
addPoint(const Eigen::Vector3f& iPoint) {
  mPoints.push_back(iPoint);
  Eigen::Vector3d p = iPoint.cast<double>();
  mSum += p;
  mSumSquared += p*p.transpose();
}
コード例 #15
0
ファイル: CrichtonView.hpp プロジェクト: ana-GT/golems
void CrichtonView<PointT>::onMouse( int event, int x, int y, int flags, void* userdata ) {
  
  if( event != cv::EVENT_LBUTTONDOWN ) {
    return;
  }
  mMutex.lock();
  cv::Point3f p; Eigen::Vector3d currentPoint;
  p = mPclMap.at<cv::Point3f>(y,x);
  currentPoint << (double)p.x, (double)p.y, (double)p.z;
  std::cout << "\t * Mouse pressed. Current point: "<< currentPoint.transpose() << std::endl;
  
  double minDist; int minIndex; double dist;

  mSelectedCluster = -1;

    for( int i = 0; i < mClustersBB.size(); ++i ) {
      int cx, cy;
      cx = (mClustersBB[i](0) + mClustersBB[i](2) )/2;
      cy = (mClustersBB[i](1) + mClustersBB[i](3) )/2;
      dist = (x-cx)*(x-cx) + (y-cy)*(y-cy);
      if( i == 0 ) { minIndex = 0; minDist = dist; }
      else { if( dist < minDist ) { minIndex = i; minDist = dist; } }
    }

    mSelectedCluster = minIndex;
    printf("\t * Selected cluster: %d \n", mSelectedCluster );
    mMutex.unlock();
}
コード例 #16
0
void VideoIMUFusion::RunningData::handleIMUVelocity(
    const OSVR_TimeValue &timestamp, const Eigen::Vector3d &angVel) {
#ifdef OSVR_FPE
    FPExceptionEnabler fpe;
#endif

    if (preReport(timestamp)) {

#if 0
        static int s = 0;
        if (s == 0) {
            static const Eigen::IOFormat fmt(3, 0, ", ", ";\n", "", "", "[",
                                             "]");
            OSVR_DEV_VERBOSE(
                "\nprediction: "
                << state().getAngularVelocity().transpose().format(fmt)
                << "\nMeasurement: " << angVel.transpose().format(fmt)
                << "\nVariance: "
                << state()
                       .errorCovariance()
                       .diagonal()
                       .tail<3>()
                       .transpose()
                       .format(fmt));
        }
        s = (s + 1) % 100;
#endif

        m_imuMeasVel.setMeasurement(angVel);
        osvr::kalman::correct(state(), processModel(), m_imuMeasVel);
    }
}
コード例 #17
0
/**
 * @function onMouse
 * @brief Tells the point location
 */
static void onMouse( int event, int x, int y, int flags, void* userdata ) {
  
  if( event != cv::EVENT_LBUTTONDOWN ) {
    return;
  }
  mutex.lock();
  cv::Point3f p; Eigen::Vector3d currentPoint;
  p = gXyzImg.at<cv::Point3f>(y,x);
  currentPoint << (double)p.x, (double)p.y, (double)p.z;
  std::cout << "\t * Mouse pressed. Current point: "<< currentPoint.transpose() << std::endl;
  
  double minDist; int minIndex; double dist;

  gSelectedCluster = -1;

    for( int i = 0; i < gClustersBB.size(); ++i ) {
      int cx, cy;
      cx = (gClustersBB[i](0) + gClustersBB[i](2) )/2;
      cy = (gClustersBB[i](1) + gClustersBB[i](3) )/2;
      dist = (x-cx)*(x-cx) + (y-cy)*(y-cy);
      if( i == 0 ) { minIndex = 0; minDist = dist; }
      else { if( dist < minDist ) { minIndex = i; minDist = dist; } }
    }

    gSelectedCluster = minIndex;

    saveData();
    mutex.unlock();
}
コード例 #18
0
ファイル: iir_notch.cpp プロジェクト: andybarry/pronto
double IIRNotch::processSample(double input){
  bool v = false;
  
  Eigen::Vector3d x_temp ( input , x(0),  x(1) );
  Eigen::Vector3d y_temp (      0, y(0),  y(1) );
  if(v)  std::cout << input << "\n";
  if(v)  std::cout << x_temp.transpose() << " x_temp\n";
  if(v)  std::cout << y_temp.transpose() << " y_temp\n";
  if(v)  std::cout << b.transpose() << " b\n";
  if(v)  std::cout << a.transpose() << " a\n";
  
  if(v){  
    Eigen::Vector3d bit =  x_temp.cross(b);
    std::cout << bit.transpose() << " bit\n";  
  }
  
  
  double output =  (x_temp.dot(b)) -  (y_temp.dot(a));
  double temp_x = x(0);
  x << input , temp_x  ;
  double temp_y = y(0);
  y <<  output, temp_y ; 
  
  if(v)  std::cout << x.transpose() << " x\n";
  if(v)  std::cout << y.transpose() << " y\n\n";
  
  return output;
}
コード例 #19
0
ファイル: PartFilter.cpp プロジェクト: cmollare/IK_PF
void PartFilter::computeDistance(int partition)
{
	std::multimap<int, std::string>::iterator it;
	for (int i=0 ; i<mModels.size() ; i++)
	{
		double distance=0;
		it = mOffsetPartToName[i].find(partition);
		for (it = mOffsetPartToName[i].equal_range(partition).first ; it != mOffsetPartToName[i].equal_range(partition).second ; ++it)
		{
			if (mJointNameToPos[(*it).second] != -1)
			{
				int pos = mJointNameToPos[(*it).second];
				double distTemp=0;
				// Mahalanobis distance
				//cout << (*it).second << "=>" << mPosNames[pos] << endl;
				Eigen::Vector3d jtPos = mModels[i]->getJoint((*it).second)->getXYZVect();
				Eigen::Vector3d jtObs(mCurrentFrame[pos][1], mCurrentFrame[pos][2], mCurrentFrame[pos][3]);
				Eigen::Vector3d diff = jtPos - jtObs;
				Eigen::Matrix3d cov;
				cov.setIdentity();
				distTemp = diff.transpose()*(cov*diff);
				distance += distTemp;
			}
		}
		mCurrentDistances[i] = distance;
	}
}
コード例 #20
0
/**
 * @brief hqp_wrapper::updObstacle Updates obstacles' matrices
 * @param levelName                 Level's ID
 * @param Jac                       Jacobian (6xNc)
 * @param n                         Normalised Vector between end effector and obstacles' position
 * @param b                         scalar b, n*J < b
 */
void hqp_wrapper::updObstacle(std::string levelName, const Eigen::MatrixXd Jac, const Eigen::Vector3d n, const double b ){
    int indx = leveNameMap[levelName];
    this->B[indx][0] = soth::Bound(b, soth::Bound::BOUND_INF);
    this->J[indx] = n.transpose()*Jac.block(0,0,3,this->NC);
    //  std::cout << "J[indx]:\n" <<J[indx] <<"\n";

}
コード例 #21
0
ファイル: filter.cpp プロジェクト: chendong/tonav
Eigen::Matrix4d Filter::omegaMatrix(const Eigen::Vector3d vec) {
    Eigen::Matrix4d mat;
    mat.block<3, 3>(0, 0) = Filter::crossMatrix(-1*vec);
    mat.block<1, 3>(3, 0) = -1*(vec.transpose());
    mat.block<3, 1>(0, 3) = vec;
    mat(3, 3) = 0;
    return mat;
}
コード例 #22
0
ファイル: motion_model.hpp プロジェクト: Yvaine/ekfoa
    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);
    }
コード例 #23
0
void generateColorModel(const PCRGB::Ptr& pc_in, Eigen::Vector3d& mean, Eigen::Matrix3d& cov)
{
    Eigen::MatrixXd X(3, pc_in->size());
    for(uint32_t i=0;i<pc_in->size();i++)
        extractHSL(pc_in->points[i].rgb, X(0,i), X(1,i), X(2,i));
    mean = X.rowwise().sum() / pc_in->size();
    cov = X * X.transpose() / pc_in->size() - mean * mean.transpose();
}
コード例 #24
0
/**
 * @brief hqp_wrapper::addObstacle
 * @param levelName         String to be used as ID
 * @param Jac               Jacobian (6xNc), only the first 3 rows (position) is used currently
 * @param n                 Normalised Vector between end effector and obstacles' position
 * @param b                 scalar b, n*J < b
 */
void hqp_wrapper::addObstacle(std::string levelName, const Eigen::MatrixXd Jac, const Eigen::Vector3d n, double b){
    Eigen::MatrixXd Jtmp;
    Jtmp = n.transpose()*Jac.block(0,0,3,this->NC);
    Eigen::Matrix<double,1,1>  B;
    B(0) = b;
    addStage(levelName,Jtmp,B,soth::Bound::BOUND_INF);
    std::cout <<"Adding new element:" << levelName <<", at:" << leveNameMap[levelName]<<"\n";
}
コード例 #25
0
void inPointsCallback(const geometry_msgs::Polygon& pts_array) {
    if (pts_array.points.size() != 2) {
        ROS_ERROR("wrong number of points in pts_array! should be two");
        return;
    }
    //unpack the points and fill in entry and exit pts
    g_O_entry_point(0) = pts_array.points[0].x;
    g_O_entry_point(1) = pts_array.points[0].y;
    g_O_entry_point(2) = pts_array.points[0].z;
    g_O_exit_point(0) = pts_array.points[1].x;
    g_O_exit_point(1) = pts_array.points[1].y;
    g_O_exit_point(2) = pts_array.points[1].z;

    g_got_new_points = true;

    cout << "received new entry point = " << g_O_entry_point.transpose() << endl;
    cout << "and exit point: " << g_O_exit_point.transpose() << endl;
}
コード例 #26
0
void inPointCallback(const geometry_msgs::Point& pt_msg) {

    g_O_entry_point(0) = pt_msg.x;
    g_O_entry_point(1) = pt_msg.y;
    g_O_entry_point(2) = pt_msg.z;
    g_got_new_entry_point = true;

    cout << "received new entry point = " << g_O_entry_point.transpose() << endl;
}
コード例 #27
0
bool ArmMotionInterface::unpack_delta_p(void) {
    int npts = request_.delta_p.size();
    if (npts != 3) {
        ROS_WARN("plan_cartesian_delta_p: request delta_p is wrong size");
        return false;
    }
    //copy message data to member var:
    for (int i = 0; i < 3; i++) delta_p_(i) = request_.delta_p[i];
    cout << "requested delta_p: " << delta_p_.transpose() << endl;
}
コード例 #28
0
ファイル: mySpring.cpp プロジェクト: jturner65/ParticleSim
	//for final project for cs7492 - implicit with conjugate gradient
	void mySpring::buildSprJpJv() {
		Eigen::Vector3d delPos = (a->getPosition() - b->getPosition());
		Eigen::Matrix3d dpdpT = delPos * delPos.transpose();

		double currLen = (delPos.norm());
		if (currLen != 0) currLen = 1.0 / currLen;     //handles if currLen is 0, Jp will be 0
		dpdpT = dpdpT*(currLen*currLen);
		Jp = (dpdpT + (Id3x3 - dpdpT)*(1 - (restLen*currLen))) * Ks;
		//Jd is constant, since kd won't change
	}
コード例 #29
0
void BoundingBox::computeOrientedBox(std::vector<Vertex>& vertices)
{
    type = "Oriented";
    orientedPoints.clear();
    
    // compute mean
    Eigen::Vector3d center;
    center.setZero();
    for (VertexCIter v = vertices.begin(); v != vertices.end(); v++) {
        center += v->position;
    }
    center /= (double)vertices.size();
    
    // adjust for mean and compute covariance
    Eigen::Matrix3d covariance;
    covariance.setZero();
    for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
        Eigen::Vector3d pAdg = v->position - center;
        covariance += pAdg * pAdg.transpose();
    }
    covariance /= (double)vertices.size();

    // compute eigenvectors for the covariance matrix
    Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
    Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();

    // project min and max points on each principal axis
    double min1 = INFINITY, max1 = -INFINITY;
    double min2 = INFINITY, max2 = -INFINITY;
    double min3 = INFINITY, max3 = -INFINITY;
    double d = 0.0;
    eigenVectors.transpose();
    for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
        d = eigenVectors.row(0).dot(v->position);
        if (min1 > d) min1 = d;
        if (max1 < d) max1 = d;
        
        d = eigenVectors.row(1).dot(v->position);
        if (min2 > d) min2 = d;
        if (max2 < d) max2 = d;
        
        d = eigenVectors.row(2).dot(v->position);
        if (min3 > d) min3 = d;
        if (max3 < d) max3 = d;
    }
    
    // add points to vector
    orientedPoints.push_back(eigenVectors.row(0) * min1);
    orientedPoints.push_back(eigenVectors.row(0) * max1);
    orientedPoints.push_back(eigenVectors.row(1) * min2);
    orientedPoints.push_back(eigenVectors.row(1) * max2);
    orientedPoints.push_back(eigenVectors.row(2) * min3);
    orientedPoints.push_back(eigenVectors.row(2) * max3);
}
コード例 #30
0
Eigen::Vector4f IncrementalPlaneEstimator::
getPlane(const Eigen::Vector3d& iSum,
         const Eigen::Matrix3d& iSumSquared,
         const double iCount) {
  Eigen::Vector3d mean = iSum/iCount;
  Eigen::Matrix3d cov = iSumSquared/iCount - mean*mean.transpose();
  Eigen::Vector4d plane;
  plane.head<3>() = cov.jacobiSvd(Eigen::ComputeFullV).matrixV().col(2);
  plane[3] = -plane.head<3>().dot(mean);
  return plane.cast<float>();
}