本文整理汇总了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 ¶ms,
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;未经允许,请勿转载。 |
请发表评论