C++ vctDynamicVector类代码示例

OGeek|极客世界-中国程序员成长平台 › 门户 › 编程› C++›C++教程

原作者: [db:作者] 来自: [db:来源] 收藏邀请

本文整理汇总了C++中vctDynamicVector类的典型用法代码示例。如果您正苦于以下问题：C++ vctDynamicVector类的具体用法？C++ vctDynamicVector怎么用？C++ vctDynamicVector使用的例子？那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。

在下文中一共展示了vctDynamicVector类的20个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于我们的系统推荐出更棒的C++代码示例。

示例1: A

// A is column major!
vctDynamicMatrix<double>
robManipulator::JSinertia( const vctDynamicVector<double>& q ) const {

  if( q.size() != links.size() ){
    CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS
                      << ": Expected " << links.size() << " values. "
                      << "Got " << q.size()
                      << std::endl;
    return vctDynamicMatrix<double>();
  }

  vctDynamicMatrix<double> A( links.size(), links.size(), 0.0 );

  for(size_t c=0; c<q.size(); c++){
    vctDynamicVector<double> qd( q.size(), 0.0 );  // velocities to zero
    vctDynamicVector<double> qdd( q.size(), 0.0 ); // accelerations to zero
    vctFixedSizeVector<double,6> fext(0.0);
    qdd[c] = 1.0;                                  // ith acceleration to 1

    vctDynamicVector<double> h = RNE( q, qd, qdd, fext, 0.0  );
    for( size_t r=0; r<links.size(); r++ )
      { A[c][r] = h[r]; }
  }

  return A;

}

开发者ID:ahundt，项目名称:cisst，代码行数:28，代码来源:robManipulator.cpp

示例2:

osaCANBusFrame::osaCANBusFrame( osaCANBusFrame::ID id, 
			      const vctDynamicVector<osaCANBusFrame::Data>& data){

  // Clear up everything before starting
  this->id = 0;                              // default ID 
  this->nbytes = 0;                          // no data
  for(osaCANBusFrame::DataLength i=0; i<8; i++) // clear the data
    { this->data[i] = 0x00; }

  // A can ID has 11 bits. Ensure that only 11 bits are used
  if( (~0x07FF) & id ){
    CMN_LOG_RUN_WARNING << CMN_LOG_DETAILS
			<< ": Illegal CAN id: " << id 
			<< std::endl;
  }

  else{
    // Now check that no more than 8 bytes are given
    if( 8 < data.size() ){
      CMN_LOG_RUN_WARNING << CMN_LOG_DETAILS
			  << ": Illegal message length: " << data.size()
			  << std::endl;
    }

    else{
      this->id = (0x07FF & id);                         // Copy the CAN ID
      this->nbytes = data.size();                       // Copy the data length
      for(osaCANBusFrame::DataLength i=0; i<nbytes; i++) // Copy the data
	{ this->data[i] = data[i]; }
    }
  }

}

开发者ID:jhu-saw，项目名称:sawCANBus，代码行数:33，代码来源:osaCANBusFrame.cpp

示例3: bins

double cisstAlgorithmICP_RobustICP::ComputeEpsilon(vctDynamicVector<double> &sampleDist)
{
  unsigned int numSamps = sampleDist.size();

  double minDist = sampleDist.MinElement();
  double maxDist = sampleDist.MaxElement();

  unsigned int numBins = 16;
  double binWidth = (maxDist - minDist) / (double)numBins;

  // build histogram of match distances
  vctDynamicVector<unsigned int> bins(numBins, (unsigned int)0);
  unsigned int sampleBin;
  for (unsigned int i = 0; i < numSamps; i++)
  {
    if (sampleDist[i] == maxDist)
    { // handle max case
      sampleBin = numBins - 1;
    }
    else
    {
      sampleBin = (unsigned int)floor((sampleDist[i] - minDist) / binWidth);
    }

    bins(sampleBin)++;
  }

  // find histogram peak
  unsigned int peakBin = numBins;  // initialize to invalid bin
  unsigned int peakBinSize = 0;
  for (unsigned int i = 0; i < numBins; i++)
  {
    if (bins(i) >= peakBinSize)
    {
      peakBin = i;
      peakBinSize = bins(i);
    }
  }
  // find valley following peak
  //  (valley bin must be <= 60% of peak bin size)  
  double valleyThresh = 0.6 * (double)peakBinSize;
  unsigned int valleyBin = peakBin + 1;
  for (unsigned int i = peakBin + 1; i < numBins; i++)
  {
    if ((double)bins(i) <= valleyThresh)
    {
      break;
    }
    valleyBin = i + 1;
  }

  // set epsilon to the smallest distance in the valley bin
  double epsilon = minDist + valleyBin * binWidth;

  //printHistogram(bins, peakBin, valleyBin, minDist, maxDist, binWidth);

  return epsilon;
}

开发者ID:mingliangfu，项目名称:IMLP，代码行数:58，代码来源:cisstAlgorithmICP_RobustICP.cpp

示例4:

osaPIDAntiWindup::Errno
osaPIDAntiWindup::Evaluate
( const vctDynamicVector<double>& qs,
  const vctDynamicVector<double>& q,
  vctDynamicVector<double>& tau,
  double dt ){

    if( dt <= 0 ){
        std::cerr << "Invalid time increment: " << dt << std::endl;
        return osaPIDAntiWindup::EFAILURE;
    }
    
    if( qs.size() != Kp.size() ){
        std::cerr << "size(qs) = " << qs.size() << " "
                          << "N = "        << Kp.size() << std::endl;
        return osaPIDAntiWindup::EFAILURE;
    }
    
    if( q.size() != Kp.size() ){
        std::cerr << "size(q) = " << q.size() << " "
                          << "N = "       << Kp.size() << std::endl;
        return osaPIDAntiWindup::EFAILURE;
    }

    // error = desired - current
    vctDynamicVector<double> e = qs - q;

    // evaluate the velocity
    vctDynamicVector<double> qd = (q - qold)/dt;
    
    // error time derivative
    vctDynamicVector<double> ed = (e - eold)/dt;      
    
    // command with anti windup
    for( size_t i=0; i<commands.size(); i++ ){
        I[i] += ( Ki[i]*e[i] + Kt[i]*(outputs[i]-commands[i]) ) * dt;
        commands[i] = Kp[i]*e[i] + Kd[i]*ed[i] + I[i];
    }

    // set the output to the command
    outputs = commands;

    // saturate the output
    for( size_t i=0; i<outputs.size(); i++ ){
        if( outputs[i] < -limits[i] ) { outputs[i] = -limits[i]; }
        if( limits[i]  < outputs[i] ) { outputs[i] =  limits[i]; }
    }
    
    tau = outputs;

    eold = e;
    qold = q;
    
    return osaPIDAntiWindup::ESUCCESS;
    
}

开发者ID:jhu-saw，项目名称:sawControllers，代码行数:56，代码来源:osaPIDAntiWindup.cpp

示例5: getFrame

void Camera::getFrame(vctDynamicVector<vct3> & points) {

	sm = PXCSenseManager::CreateInstance();
	PXCCaptureManager *cm = sm->QueryCaptureManager();

	sm->EnableStream(PXCCapture::STREAM_TYPE_DEPTH, 640, 480, 30);

	pxcStatus sts = sm->Init();
	if (sts < PXC_STATUS_NO_ERROR) {
		std::cout << "DIDN\'T WORK" << std::endl;
	}

	PXCCapture::Device *device = sm->QueryCaptureManager()->QueryDevice();
	device->ResetProperties(PXCCapture::STREAM_TYPE_ANY);

	std::cout << device->SetDepthUnit(1000) << std::endl;

	PXCProjection *projection = device->CreateProjection();


	pxcStatus sts2 = sm->AcquireFrame(false);
	if (sts2 >= PXC_STATUS_NO_ERROR) {
		PXCCapture::Sample *sample = sm->QuerySample();
		PXCImage* image = (*sample)[streamType];
		PXCImage::ImageInfo info = {};
		PXCImage::ImageData data;
		image->AcquireAccess(PXCImage::ACCESS_READ, &data);
		PXCImage::ImageInfo dinfo = sample->depth->QueryInfo();
		int dpitch = data.pitches[0] / sizeof(short);
		short *dpixels = (short*)data.planes[0];
		int i = 0;
		points.SetSize(dinfo.width * dinfo.height);

		PXCPoint3DF32 * vertices = new PXCPoint3DF32[dinfo.width * dinfo.height];
		projection->QueryVertices(image, vertices);

		int c = 0;
		for (int i = 0; i < points.size(); i++) {
			PXCPoint3DF32 point = vertices[i];
			if (point.z != 0) {
				vct3 newPoint(point.x, point.y, point.z);
				points[c++] = newPoint;
			}
		}
		points.resize(c);
		image->ReleaseAccess(&data);
	}
	projection->Release();
	std::cout << sts2 << std::endl;
}

开发者ID:superchao1982，项目名称:EyeInHand，代码行数:50，代码来源:Camera.cpp

示例6: robManipulator

osaPDGC::osaPDGC( const std::string& robfile,
                  const vctFrame4x4<double>& Rtw0,
                  const vctDynamicMatrix<double>& Kp,
                  const vctDynamicMatrix<double>& Kd,
                  const vctDynamicVector<double>& qinit ):
    robManipulator( robfile, Rtw0 ),
    Kp( Kp ),
    Kd( Kd ),
    qold( qinit ),
    eold( qinit.size(), 0.0 ) {

    if( Kp.rows() != links.size() || Kp.cols() != links.size() ) {
        CMN_LOG_RUN_ERROR << "size(Kp) = [" << Kp.rows()
                          << ", "           << Kp.cols() << " ] "
                          << "NxN = ["      << links.size()
                          << ", "           << links.size() << " ]"
                          << std::endl;
    }

    if( Kd.rows() != links.size() || Kd.cols() != links.size() ) {
        CMN_LOG_RUN_ERROR << "size(Kd) = [" << Kd.rows()
                          << ", "           << Kd.cols() << " ] "
                          << "NxN = ["      << links.size()
                          << ", "           << links.size() << " ]"
                          << std::endl;
    }

    if( qold.size() != links.size() ) {
        CMN_LOG_RUN_ERROR << "size(qold) = " << qold.size() << " "
                          << "N = "          << links.size() << std::endl;
    }

}

开发者ID:jhu-saw，项目名称:sawControllers，代码行数:33，代码来源:osaPDGC.cpp

示例7: Kp

osaPIDAntiWindup::osaPIDAntiWindup( const vctDynamicVector<double>& Kp,
                                    const vctDynamicVector<double>& Ki,
                                    const vctDynamicVector<double>& Kd,
                                    const vctDynamicVector<double>& Kt,
                                    const vctDynamicVector<double>& limits,
                                    const vctDynamicVector<double>& qinit ) :
    Kp( Kp ),
    Ki( Ki ),
    Kd( Kd ),
    Kt( Kt ),
    I( Ki.size(), 0.0 ),
    commands( limits.size(), 0.0 ),
    outputs( limits.size(), 0.0 ),
    limits( limits.size(), 0.0 ),
    qold( qinit ),
    eold( qinit.size(), 0.0 ){

    if( this->Kp.size() != this->Kd.size() || 
        this->Kp.size() != this->Ki.size() || 
        this->Kp.size() != this->Kt.size() ){
        std::cerr << "Size mismatch:" 
                          << " size(Kp) = " << this->Kp.size()
                          << " size(Ki) = " << this->Ki.size()
                          << " size(Kd) = " << this->Kd.size()
                          << " size(Kt) = " << this->Kt.size()
                          << std::endl;
    }

    if( this->Kp.size() != this->limits.size() ){
        std::cerr << "Size mismatch: initializing " << this->Kp.size() 
                          << " controllers with " << this->limits.size() 
                          << " limit values."
                          << std::endl;
    }

    if( this->Kp.size() != this->qold.size() ){
        std::cerr << "Size mismatch: initializing " << this->Kp.size() 
                          << " controllers with " << this->qold.size() 
                          << " values."
                          << std::endl;
    }

    for( size_t i=0; i<this->limits.size(); i++ )
        { this->limits[i] = fabs( limits[i] ); }

}

开发者ID:jhu-saw，项目名称:sawControllers，代码行数:46，代码来源:osaPIDAntiWindup.cpp

示例8: RNE

vctDynamicVector<double>
robManipulator::CCG( const vctDynamicVector<double>& q,
                     const vctDynamicVector<double>& qd ) const {

  if( q.size() != qd.size() ){
    CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS
                      << ": Size of q and qd do not match."
                      << std::endl;
    return vctDynamicVector<double>();
  }



  return RNE( q,           // call Newton-Euler with only the joints positions
          qd,          // and the joints velocities
              vctDynamicVector<double>( q.size(), 0.0 ),
              vctFixedSizeVector<double,6>(0.0) );
}

开发者ID:ahundt，项目名称:cisst，代码行数:18，代码来源:robManipulator.cpp

示例9: SetSampleCovariances

void cisstAlgorithmICP_IMLP::SetSampleCovariances(
  vctDynamicVector<vct3x3> &Mi,
  vctDynamicVector<vct3x3> &MsmtMi)
{
  if (Mi.size() != nSamples || MsmtMi.size() != nSamples)
  {
    std::cout << "ERROR: number of covariances matrices does not match number of samples" << std::endl;
  }

  Mxi.SetSize(nSamples);
  eigMxi.SetSize(nSamples);
  for (unsigned int s = 0; s<nSamples; s++)
  {
    Mxi[s] = Mi[s];
    ComputeCovEigenValues_SVD(Mi[s], eigMxi[s]);
  }

  this->MsmtMxi = MsmtMi;
}

开发者ID:mingliangfu，项目名称:IMLP，代码行数:19，代码来源:cisstAlgorithmICP_IMLP.cpp

示例10: qd

osaPDGC::Errno
osaPDGC::Evaluate
( const vctDynamicVector<double>& qs,
  const vctDynamicVector<double>& q,
  vctDynamicVector<double>& tau,
  double dt ) {

    if( qs.size() != links.size() ) {
        CMN_LOG_RUN_ERROR << "size(qs) = " << qs.size() << " "
                          << "N = "        << links.size() << std::endl;
        return osaPDGC::EFAILURE;
    }

    if( q.size() != links.size() ) {
        CMN_LOG_RUN_ERROR << "size(q) = " << q.size() << " "
                          << "N = "       << links.size() << std::endl;
        return osaPDGC::EFAILURE;
    }

    // evaluate the velocity
    vctDynamicVector<double> qd( links.size(), 0.0 );
    if( 0 < dt ) qd = (q - qold)/dt;

    // error = current - desired
    vctDynamicVector<double> e = q - qs;

    // error time derivative
    vctDynamicVector<double> ed( links.size(), 0.0 );
    if( 0 < dt ) ed = (e - eold)/dt;

    // Compute the coriolis+gravity load
    vctDynamicVector<double> ccg =
        CCG( q, vctDynamicVector<double>( links.size(), 0.0 ) );

    tau = ccg - Kp*e - Kd*ed;

    eold = e;
    qold = q;

    return osaPDGC::ESUCCESS;

}

开发者ID:jhu-saw，项目名称:sawControllers，代码行数:42，代码来源:osaPDGC.cpp

示例11: prmJointTypeToFactor

void prmJointTypeToFactor(const vctDynamicVector<prmJointType> & types,
                          const double prismaticFactor,
                          const double revoluteFactor,
                          vctDynamicVector<double> & factors)
{
    // set unitFactor;
    for (size_t i = 0; i < types.size(); i++) {
        switch (types.at(i)) {
        case PRM_JOINT_PRISMATIC:
            factors.at(i) = prismaticFactor;
            break;
        case PRM_JOINT_REVOLUTE:
            factors.at(i) = revoluteFactor;
            break;
        default:
            factors.at(i) = 1.0;
            break;
        }
    }
}

开发者ID:jhu-cisst，项目名称:cisst，代码行数:20，代码来源:prmJointType.cpp

示例12:

vctFrame4x4<double>
robManipulator::ForwardKinematics( const vctDynamicVector<double>& q,
                                   int N ) const {

  if( N == 0 ) return Rtw0;

  // if N < 0 then we want the end-effector
  if( N < 0 ) N = links.size();

  if( ((int)q.size()) < N ){
    CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS
                      << ": Expected " << N << " joint positions but "
                      << "size(q) = " << q.size() << "."
                      << std::endl;
    return Rtw0;
  }

  // no link? then return the transformation of the base
  if( links.empty() ){
    //CMN_LOG_RUN_WARNING << CMN_LOG_DETAILS
    //                << ": Manipulator has no link."
    //                        << std::endl;
    return Rtw0;
  }

  // set the position/orientation of link 0 to the base * its tranformation
  // setting the link's transformation is necessary in order to render the link
  // in opengl
  vctFrame4x4<double> Rtwi = Rtw0*links[0].ForwardKinematics( q[0] );

  // for link 1 to N
  for(int i=1; i<N; i++)
    Rtwi = Rtwi * links[i].ForwardKinematics( q[i] );

  if( tools.size() == 1 ){
    if( tools[0] != NULL )
      { return Rtwi * tools[0]->ForwardKinematics( q, 0 ); }
  }

  return Rtwi;
}

开发者ID:ahundt，项目名称:cisst，代码行数:41，代码来源:robManipulator.cpp

示例13: JOBZ

nmrSymmetricEigenProblem::Data::Data( vctDynamicMatrix<double>& A,
				      vctDynamicVector<double>& D,
				      vctDynamicMatrix<double>& V ) :
  JOBZ( 'V' ),
  RANGE( 'A' ),
  UPLO( 'U' ),

  N( A.rows() ),
  A( A.Pointer() ),
  LDA( A.cols() ),

  VL( 0 ),
  VU( 0 ),
  IL( 0 ),
  IU( 0 ),

  DLAMCH( 'S' ),
  ABSTOL( dlamch_( &DLAMCH ) ),
  
  W( D.Pointer() ),
  Z( V.Pointer() ),
  LDZ( V.cols() ),
  
  ISUPPZ( new CISSTNETLIB_INTEGER[ 2*N ] ),
  WORK( NULL ),
  LWORK( -1 ),

  IWORK( NULL ),
  LIWORK( -1 ){

  CheckSystem( A, D, V );

  CISSTNETLIB_DOUBLE work;
  CISSTNETLIB_INTEGER iwork;

  dsyevr_( &JOBZ, &RANGE, &UPLO,
	   &N, this->A, &LDA, 
	   &VL, &VU, 
	   &IL, &IU, 
	   &ABSTOL, 
	   &M, W, 
	   Z, &LDZ, ISUPPZ,
	   &work, &LWORK,
	   &iwork, &LIWORK, 
	   &INFO );

  LWORK = work;
  WORK = new CISSTNETLIB_DOUBLE[LWORK];

  LIWORK = iwork;
  IWORK = new CISSTNETLIB_INTEGER[LIWORK];
  
}

开发者ID:Shuyoung，项目名称:cisst，代码行数:53，代码来源:nmrSymmetricEigenProblem.cpp

示例14: message

void 
nmrSymmetricEigenProblem::Data::CheckSystem
( const vctDynamicMatrix<double>& A,
  const vctDynamicVector<double>& D,
  const vctDynamicMatrix<double>& V ){

  // test that number of rows match
  if( A.rows() != A.cols() ){
    std::ostringstream message;
    message << "nmrSymmetricEigenProblem: Matrix A has " << A.rows() << " rows "
	    << " and "  << A.cols() << " columns.";
    cmnThrow( std::runtime_error( message.str() ) );
  }

  // check for fortran format
  if( !A.IsFortran() ){
    std::string message( "nmrSymmetricEigenProblem: Invalid matrix A format." );
    cmnThrow( std::runtime_error( message ) );
  }

  // test that number of rows match
  if( V.rows() != A.rows() || V.cols() != A.cols() ){
    std::ostringstream message;
    message << "nmrSymmetricEigenProblem: Matrix V has " << V.rows() << " rows "
	    << " and "  << V.cols() << " columns.";
    cmnThrow( std::runtime_error( message.str() ) );
  }

  if( !V.IsFortran() ){
    std::string message( "nmrSymmetricEigenProblem: Invalid matrix V format." );
    cmnThrow( std::runtime_error( message ) );
  }

  if( D.size() != A.rows() ){
    std::ostringstream message;
    message << "nmrSymmetricEigenProblem: Vector D has " << D.size() << " rows";
    cmnThrow( std::runtime_error( message.str() ) );
  }

}

开发者ID:Shuyoung，项目名称:cisst，代码行数:40，代码来源:nmrSymmetricEigenProblem.cpp

示例15: qd

void robManipulator::JSinertia( double **A,
                                const vctDynamicVector<double>& q ) const {

  if( q.size() != links.size() ){
    std::cerr << "robManipulator::JSinertia: Expected " << links.size()
              << " values. Got " << q.size()
              << std::endl;
    return;
  }

  for(size_t c=0; c<links.size(); c++){
    vctDynamicVector<double> qd( q.size(), 0.0 ); // velocities to zero
    vctDynamicVector<double> qdd(q.size(), 0.0 ); // accelerations to zero
    vctFixedSizeVector<double,6> fext(0.0);
    qdd[c] = 1.0;                                 // ith acceleration to 1

    vctDynamicVector<double> h = RNE( q, qd, qdd, fext, 0  );
    for( size_t r=0; r<links.size(); r++ )
      A[c][r] = h[r];
  }

}

开发者ID:ahundt，项目名称:cisst，代码行数:22，代码来源:robManipulator.cpp

示例16: printHistogram

void cisstAlgorithmICP_RobustICP::printHistogram(
  vctDynamicVector<unsigned int> bins, 
  unsigned int peakBin, unsigned int valleyBin,
  double minDist, double maxDist,
  double binWidth)
{
  std::stringstream ss;
  
  unsigned int numBins = bins.size();

  ss << std::endl << "Match Distance Histogram:" << std::endl;
  ss << "minDist: " << minDist << std::endl;
  ss << "maxDist: " << maxDist << std::endl;
  ss << std::endl;

  // bin header
  ss << " bins | size | dist " << std::endl;
  for (unsigned int i = 0; i < numBins; i++)
  {
    ss << " "; ss.width(4); ss << i;
    ss << " | "; ss.width(4); ss << bins(i);
    ss << " | " << minDist + i*binWidth << " ";

    if (i == peakBin)
    {
      ss << " <--- peak bin";
    }
    if (i == valleyBin)
    {
      ss << " <--- valley bin";
    }

    ss << std::endl;
  }
  ss << std::endl;

  //// bin contents
  //ss << "      ";
  //for (unsigned int i = 0; i < numBins; i++)
  //{
  //  ss << "| "; ss.width(4); ss << bins(i) << " ";
  //}
  //ss << std::endl;

  std::cout << ss.str() << std::endl;

}

开发者ID:mingliangfu，项目名称:IMLP，代码行数:47，代码来源:cisstAlgorithmICP_RobustICP.cpp

示例17: CCG

vctDynamicVector<double>
robManipulator::InverseDynamics( const vctDynamicVector<double>& q,
                                 const vctDynamicVector<double>& qd,
                                 const vctDynamicVector<double>& qdd ) const {

  vctDynamicVector<double> ccg = CCG(q, qd);

  doublereal* Inertterm  = new doublereal[ links.size() ];
  for( size_t i=0; i<links.size(); i++ )
    Inertterm [i] = 0;

  // Ensure there's an acceleration. Orthewise only consider ccg
  if( 0.0 < qdd.Norm() ){

    char LOW = 'L';
    integer N = links.size();
    integer LDA = links.size();
    integer INC = 1;
    doublereal ALPHA = 1.0;
    doublereal BETA = 0.0;

    doublereal** A = rmatrix(0, links.size()-1, 0, links.size()-1);
    JSinertia(A,q);

    // must copy to an array for symv
    doublereal* qddv = new doublereal[ links.size() ];
    for( size_t i=0; i<links.size(); i++ ) qddv[i] = qdd[i];

    symv(&LOW, &N,
         &ALPHA, &A[0][0], &LDA,
         qddv, &INC,
         &BETA, Inertterm, &INC);

    free_rmatrix(A, 0, 0);
    delete[] qddv;
  }

  // add inertia+coriolis+grav
  vctDynamicVector<double> trq( links.size(), 0.0 );
  for( size_t i=0; i<links.size(); i++ )
    trq[i] = Inertterm[i] + ccg[i];

  delete[] Inertterm;

  return trq;
}

开发者ID:ahundt，项目名称:cisst，代码行数:46，代码来源:robManipulator.cpp

示例18: GenerateSamplePointSet_PointCloud_SurfaceNoise

void GenerateSamplePointSet_PointCloud_SurfaceNoise(
  cisstMesh &mesh,
  TestParameters &params,
  unsigned int randSeed, unsigned int &randSeqPos,
  std::ifstream &randnStream,
  vctDynamicVector<vct3> &samples,
  vctDynamicVector<vct3> &sampleNorms,
  vctDynamicVector<vct3> &noisySamples,
  unsigned int trialNum)
{
  // get custom params
  CustomTestParams_PointCloud_SurfaceNoise *pCustomParams =
    dynamic_cast<CustomTestParams_PointCloud_SurfaceNoise*>(params.pCustomTestParams);

  std::stringstream saveSamplesPath;
  std::stringstream saveNoisySamplesPath;
  std::stringstream saveNoiseCovPath;

  saveSamplesPath << params.outputCommonDir << "/SaveSamples_" << trialNum << ".pts";
  saveNoisySamplesPath << params.outputCommonDir << "/SaveNoisySamples_" << trialNum << ".pts";
  saveNoiseCovPath << params.outputCommonDir << "/SaveNoiseCov_" << trialNum << ".txt";

  std::string  strSaveSamplesPath = saveSamplesPath.str();
  std::string  strSaveNoisySamplesPath = saveNoisySamplesPath.str();
  std::string  strSaveNoiseCovPath = saveNoiseCovPath.str();

  std::string *pSaveSamplesPath = &strSaveSamplesPath;
  std::string *pSaveNoisySamplesPath = &strSaveNoisySamplesPath;
  std::string *pSaveNoiseCovPath = &strSaveNoiseCovPath;
#ifdef DISABLE_OUTPUT_FILES
  pSaveSamplesPath = NULL;
  pSaveNoisySamplesPath = NULL;
  pSaveNoiseCovPath = NULL;
#endif

  vctDynamicVector<unsigned int>  sampleDatums(params.nSamples);
  vctDynamicVector<vct3x3>        sampleCov(params.nSamples, vct3x3(0.0));
  vctDynamicVector<vct3x3>        noiseCov(params.nSamples, vct3x3(0.0));
  vctDynamicVector<vct3x3>        surfaceModelCov(params.nSamples, vct3x3(0.0));
  //vctDynamicVector<vct3x3>        noiseInvCov(params.nSamples, vct3x3(0.0));

  // Generate Samples
  GenerateSamples(
    mesh,
    randSeed, randSeqPos,
    params.nSamples,
    samples, sampleNorms, sampleDatums, 
    pSaveSamplesPath);

  // Generate Sample Noise
  GenerateSampleErrors_SurfaceNoise(
    randSeed, randSeqPos, randnStream,
    pCustomParams->sampleNoise_InPlaneSD, pCustomParams->sampleNoise_PerpPlaneSD,
    samples, sampleNorms,
    noisySamples,
    noiseCov, //noiseInvCov,
    pCustomParams->percentOutliers,
    pCustomParams->minPosOffsetOutlier, pCustomParams->maxPosOffsetOutlier, 
    pSaveNoisySamplesPath,
    pSaveNoiseCovPath);

  // Set Samples in Algorithm
  switch (params.algType)
  {
  case TestParameters::StdICP:
  {
    // configure algorithm samples
    cisstAlgorithmICP_StdICP *pAlg = dynamic_cast<cisstAlgorithmICP_StdICP*>(params.pAlg);
    pAlg->SetSamples(noisySamples);
    break;
  }

  case TestParameters::IMLP:
  {
    std::stringstream saveSampleCovPath;
    std::stringstream saveSurfaceModelCovPath;
    saveSampleCovPath << params.outputDataDir << "/SaveSampleCov_" << trialNum << ".txt";
    saveSurfaceModelCovPath << params.outputDataDir << "/SaveSurfaceModelCov_" << trialNum << ".txt";

    std::string strSaveSampleCovPath = saveSampleCovPath.str();
    std::string strSaveSurfaceModelCovPath = saveSurfaceModelCovPath.str();

    std::string *pSaveSampleCovPath = &strSaveSampleCovPath;
    std::string *pSaveSurfaceModelCovPath = &strSaveSurfaceModelCovPath;
#ifdef MINIMIZE_OUTPUT_FILES
    pSaveSampleCovPath = NULL;
    pSaveSurfaceModelCovPath = NULL;
#endif
#ifdef DISABLE_OUTPUT_FILES
    pSaveSampleCovPath = NULL;
    pSaveSurfaceModelCovPath = NULL;
#endif

    unsigned int nSamps = samples.size();

    // Set Sample Covariances
    // noise covariance
    if (pCustomParams->bSampleCov_ApplyNoiseModel)
    {
      for (unsigned int i = 0; i < nSamps; i++) 
//.........这里部分代码省略.........

开发者ID:mingliangfu，项目名称:IMLP，代码行数:101，代码来源:ParameterizedTest_PointCloud_SurfaceNoise.cpp

示例19: ForwardKinematics

robManipulator::Errno
robManipulator::InverseKinematics( vctDynamicVector<double>& q,
                                   const vctFrame4x4<double>& Rts,
                                   double tolerance,
                                   size_t Niterations,
                                   double LAMBDA ){

  if( q.size() != links.size() ){
    CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS
                      << ": Expected " << links.size() << " joints values. "
                      << " Got " << q.size()
                      << std::endl;
    return robManipulator::EFAILURE;
  }

  if( links.size() == 0 ){
    CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS
                      << ": The manipulator has no links."
                      << std::endl;
    return robManipulator::EFAILURE;
  }

  // A is a pointer to the 6xN spatial Jacobian
  integer M = 6;                  // The number or rows of matrix A
  integer N = links.size();       // The number of columns of matrix A
  integer NRHS = 1;               // The number of right hand side vectors

  integer LDA = M;                // The leading dimension of the array A.

  // B is a pointer the the N vector containing the solution
  doublereal* B;
  if( N < 6 )
    { B = new doublereal[6]; }   // The N-by-NRHS matrix of
  else
    { B = new doublereal[N]; }

  integer LDB = N;                // The leading dimension of the array B.

  // These values are used for the SVD computation
  integer INFO;                   // The info code
  integer INC = 1;                // The index increment

  doublereal ndq=1;               // norm of the iteration error
  size_t i;                   // the iteration counter
  char TRANSN = 'N';          // "N"ormal
  char TRANST = 'T';          // "T"transpose
  doublereal ALPHA = 1.0;
  doublereal* dq = new doublereal[N];

  // loop until Niter are executed or the error is bellow the tolerance
  for( i=0; i<Niterations && tolerance<ndq; i++ ){

    // Evaluate the forward kinematics
    vctFrame4x4<double,VCT_ROW_MAJOR> Rt = ForwardKinematics( q );
    // Evaluate the spatial Jacobian (also evaluate the forward kin)
    JacobianSpatial( q );

    // compute the translation error
    vctFixedSizeVector<double,3> dt( Rts[0][3]-Rt[0][3],
                                     Rts[1][3]-Rt[1][3],
                                     Rts[2][3]-Rt[2][3] );

    // compute the orientation error
    // first build the [ n o a ] vectors
    vctFixedSizeVector<double,3> n1( Rt[0][0],  Rt[1][0],  Rt[2][0] );
    vctFixedSizeVector<double,3> o1( Rt[0][1],  Rt[1][1],  Rt[2][1] );
    vctFixedSizeVector<double,3> a1( Rt[0][2],  Rt[1][2],  Rt[2][2] );
    vctFixedSizeVector<double,3> n2( Rts[0][0], Rts[1][0], Rts[2][0] );
    vctFixedSizeVector<double,3> o2( Rts[0][1], Rts[1][1], Rts[2][1] );
    vctFixedSizeVector<double,3> a2( Rts[0][2], Rts[1][2], Rts[2][2] );

    // This is the orientation error
    vctFixedSizeVector<double,3> dr = 0.5*( (n1%n2) + (o1%o2) + (a1%a2) );

    // combine both errors in one R^6 vector
    doublereal e[6] = { dt[0], dt[1], dt[2], dr[0], dr[1], dr[2] };

    //
    for( integer j=0; j<N; j++ )
      { B[j] = 0.0; }
    for( integer j=0; j<6; j++ )
      { B[j] = e[j]; }

    // weights
    doublereal I[6][6] = { { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
                           { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0 },
                           { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0 },
                           { 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 },
                           { 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 },
                           { 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 } };

    // We need to solve dq = J' ( JJ' + lambda I )^-1 e

    // I = JJ' + lambda I
    gemm( &TRANSN, &TRANST, &M, &M, &N,
          &ALPHA,
          &Js[0][0], &LDA,
          &Js[0][0], &LDA,
          &LAMBDA,
          &(I[0][0]), &M );
//.........这里部分代码省略.........

开发者ID:ahundt，项目名称:cisst，代码行数:101，代码来源:robManipulator.cpp

示例20: rank

robManipulator::Errno
robManipulator::InverseKinematics( vctDynamicVector<double>& q,
                                   const vctFrame4x4<double>& Rts,
                                   double tolerance,
                                   size_t Niterations ){

  if( q.size() != links.size() ){
    CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS
                      << ": Expected " << links.size() << " joints values. "
                      << " Got " << q.size()
                      << std::endl;
    return robManipulator::EFAILURE;
  }

  if( links.size() == 0 ){
    CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS
                      << ": The manipulator has no links."
                      << std::endl;
    return robManipulator::EFAILURE;
  }

  // A is a pointer to the 6xN spatial Jacobian
  int M = 6;                  // The number of rows of matrix A
  int N = links.size();       // The number of columns of matrix A
  int NHRS = 1;               // The number of right hand side vectors

  double* A = &(Js[0][0]);    // Pointer to the spatial Jacobian
  int LDA = M;                // The leading dimension of the array A.

  // B is a pointer the the N vector containing the solution
  double* B;                  // The N-by-NRHS matrix of right hand side matrix
  int LDB;                    // The leading dimension of the array B.
  if( N < 6 )    { B = new double[6]; LDB = 6; }
  else           { B = new double[N]; LDB = N; }

  // These values are used for the SVD computation
  double* S = new double[M];  // The singular values of A in decreasing order
  double RCOND = -1;          // Use machine precision to determine rank(A)
  int RANK;                   // The effective rank of A
  int LWORK = 128;            // The (safe) size of the workspace
  double WORK[128];           // The workspace
  int INFO;                   // The info code
  int INC = 1;                // The index increment

  double ndq=1;               // norm of the iteration error
  size_t i = 0;               // the iteration counter

  // loop until Niter are executed or the error is bellow the tolerance
  for( i=0; i<Niterations && tolerance<ndq; i++ ){

    // Evaluate the forward kinematics
    vctFrame4x4<double,VCT_ROW_MAJOR> Rt( ForwardKinematics( q ) );
    // Evaluate the spatial Jacobian (also evaluate the forward kin)
    JacobianSpatial( q );

    // compute the translation error
    vctFixedSizeVector<double,3> dt( Rts[0][3]-Rt[0][3],
                                     Rts[1][3]-Rt[1][3],
                                     Rts[2][3]-Rt[2][3] );

    // compute the orientation error
    // first build the [ n o a ] vectors
    vctFixedSizeVector<double,3> n1( Rt[0][0],  Rt[1][0],  Rt[2][0] );
    vctFixedSizeVector<double,3> o1( Rt[0][1],  Rt[1][1],  Rt[2][1] );
    vctFixedSizeVector<double,3> a1( Rt[0][2],  Rt[1][2],  Rt[2][2] );
    vctFixedSizeVector<double,3> n2( Rts[0][0], Rts[1][0], Rts[2][0] );
    vctFixedSizeVector<double,3> o2( Rts[0][1], Rts[1][1], Rts[2][1] );
    vctFixedSizeVector<double,3> a2( Rts[0][2], Rts[1][2], Rts[2][2] );

    // This is the orientation error
    vctFixedSizeVector<double,3> dr = 0.5*( (n1%n2) + (o1%o2) + (a1%a2) );

    // combine both errors in one R^6 vector
    double e[6] = { dt[0], dt[1], dt[2], dr[0], dr[1], dr[2] };

    // get a pointer
    for( int j=0; j<N; j++ )
      { B[j] = 0.0; }

    for( int j=0; j<6; j++ )
      { B[j] = e[j]; }

    // compute the minimum norm solution
    gelss( &M, &N, &NHRS,       // 6xN matrix
           A, &LDA,             // Jacobian matrix
           B, &LDB,             // error vector
           S, &RCOND, &RANK,    // SVD parameters
           WORK, &LWORK, &INFO );

    // process the errors (if any)
    if(INFO < 0)
      CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS
                        << ": the i-th argument of gelss is illegal."
                        << std::endl;
    if(0 < INFO)
      CMN_LOG_RUN_ERROR << CMN_LOG_DETAILS
                        << ": gelss failed to converge."
                        << std::endl;

    // compute the L2 norm of the error
//.........这里部分代码省略.........

开发者ID:ahundt，项目名称:cisst，代码行数:101，代码来源:robManipulator.cpp

注：本文中的vctDynamicVector类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。

鲜花

握手

雷人

路过

鸡蛋

该文章已有0人参与评论

请发表评论

全部评论

专题导读

More+

10-27 六六分期app的软件客服如何联系？(六六分期

11-06 可心卡盟:win10系统火狐flash插件崩溃怎么

11-06 亲亲特价:怎么删除回收站图标

11-06 济南大学虚拟社区:鲁大师节能降温的具体办

11-06 xlueops.exe:无线网络安装向导

11-06 女斗合众国:win7系统cf与主机连接不稳定怎

11-06 0xc000022-[cf烟雾头]cf怎么调烟雾头

11-06 qizideyouhuo:应用程序无法正常启动0xc0000

11-06 ipz-185:win7系统vcf文件怎么打开

11-06 傻哥蹦迪:win10系统s4怎么打开usb调试

11-06 八神浩树gtaste:回收站清空了怎么恢复

11-06 妖尾之黑色守护:win10系统电脑没有1440x900

11-06 校园至尊魔王小说:win7系统浏览网页时字体

11-06 女斗合众国:win10系统访问共享文件夹提示请

11-06 tokyo hot n0654:恢复win7系统默认字体一招

11-06 雨酷仙境:设置win7系统转移临时文件夹腾出

11-06 阿穆纳伊之杖:win7系统开始菜单在右边还原

11-06 tunespotting:win10系统火狐flash插件总是

11-06 甘尔葛分析师：计谋网站seo关键词暴涨有什

11-06 蔡贵霖: 计谋网站seo关键词暴涨有什么秘密

11-06 博益网首页:ao3网页版进入不了解决方法

11-06 漏斗子专栏: 网站数据分析小白易懂精华篇

11-06 见证双虹怎么做:win7系统开启telnet命令的

11-06 颾狐蝶蜋:系统资源不足无法完成请求的服务

11-06 国光中学校歌:提交网站到alexa查询详细步骤

11-06 西安有情天:静态网页和动态网页的区别

11-06 红木雅尚斋:外部链接构造对网站的好处

11-06 前官礼遇：防止域名劫持–增强域安全性的10

11-06 密传二转答案: 中文分词算法有哪些

11-06 金泉家园邮编:百度快照劫持的表现及应对方

C++ vd类代码示例发布时间：2022-05-31

C++ vconfig类代码示例发布时间：2022-05-31

剪的笔顺,诠释剪的笔画,认识剪的部首

1 六六分期app的软件客服如何联系？(六六分期

六六分期app的软件客服如何联系？不知道吗？加qq群【895510560】即可！标题：六六分期

阅读：18229|2023-10-27

2 可心卡盟:win10系统火狐flash插件崩溃怎么

今天小编告诉大家如何处理win10系统火狐flash插件总是崩溃的问题，可能很多用户都不知

阅读：9664|2022-11-06

3 亲亲特价:怎么删除回收站图标

今天小编告诉大家如何对win10系统删除桌面回收站图标进行设置，可能很多用户都不知道

阅读：8174|2022-11-06

4 济南大学虚拟社区:鲁大师节能降温的具体办

今天小编告诉大家如何对win10系统电脑设置节能降温的设置方法，想必大家都遇到过需要

阅读：8546|2022-11-06

5 xlueops.exe:无线网络安装向导

我们在使用xp系统的过程中,经常需要对xp系统无线网络安装向导设置进行设置，可能很多

阅读：8452|2022-11-06

6 女斗合众国:win7系统cf与主机连接不稳定怎

今天小编告诉大家如何处理win7系统玩cf老是与主机连接不稳定的问题，可能很多用户都不

阅读：9377|2022-11-06

7 0xc000022-[cf烟雾头]cf怎么调烟雾头

电脑对日常生活的重要性小编就不多说了，可是一旦碰到win7系统设置cf烟雾头的问题，很

阅读：8422|2022-11-06

8 qizideyouhuo:应用程序无法正常启动0xc0000

我们在日常使用电脑的时候，有的小伙伴们可能在打开应用的时候会遇见提示应用程序无法

阅读：7855|2022-11-06

9 ipz-185:win7系统vcf文件怎么打开

今天小编告诉大家如何对win7系统打开vcf文件进行设置，可能很多用户都不知道怎么对win

阅读：8407|2022-11-06

10 傻哥蹦迪:win10系统s4怎么打开usb调试

今天小编告诉大家如何对win10系统s4开启USB调试模式进行设置，可能很多用户都不知道怎

阅读：7393|2022-11-06

客服电话

电子邮件

C++ vctDynamicVector类代码示例

示例1: A

示例2:

示例3: bins

示例4:

示例5: getFrame

示例6: robManipulator

示例7: Kp

示例8: RNE

示例9: SetSampleCovariances

示例10: qd

示例11: prmJointTypeToFactor

示例12:

示例13: JOBZ

示例14: message

示例15: qd

示例16: printHistogram

示例17: CCG

示例18: GenerateSamplePointSet_PointCloud_SurfaceNoise

示例19: ForwardKinematics

示例20: rank

请发表评论

全部评论

上一篇：

下一篇：

hackl/tikz-network: A tool to visualize

krishnaik06/Machine-Learning-in-90-days

joaomh/curso-de-matlab

美元符号为什么是“$”

rugk/mastodon-simplified-federation: Sim

剪的笔顺,诠释剪的笔画,认识剪的部首

六六分期app的软件客服如何联系？(六六分期

florent37/ViewAnimator: A fluent Android

florent37/Shrine-MaterialDesign2: implem

CVE-2020-36276

SimpleSoftwareIO/simple-sms: Send and re

关于我们

产品与服务

解决方案

139-2527-9053