C++ Vector3d::transpose方法代码示例

/// 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();
	}
}

/*
 * 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);
}

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);
}

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;
}

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()});
}

/// 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;
}

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();

}

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;

}

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;
}

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;

}

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;
}

// 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());
}

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);
}

void IncrementalPlaneEstimator::
addPoint(const Eigen::Vector3f& iPoint) {
  mPoints.push_back(iPoint);
  Eigen::Vector3d p = iPoint.cast<double>();
  mSum += p;
  mSumSquared += p*p.transpose();
}

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();
}