本文整理汇总了C++中vpHomogeneousMatrix::insert方法的典型用法代码示例。如果您正苦于以下问题：C++ vpHomogeneousMatrix::insert方法的具体用法？C++ vpHomogeneousMatrix::insert怎么用？C++ vpHomogeneousMatrix::insert使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类vpHomogeneousMatrix的用法示例。
在下文中一共展示了vpHomogeneousMatrix::insert方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: u
/*!
\brief Compute the pose using the Lowe non linear approach
it consider the minimization of a residual using
the levenberg marquartd approach.
The approach has been proposed by D.G Lowe in 1992 paper \cite Lowe92a.
*/
void
vpPose::poseLowe(vpHomogeneousMatrix & cMo)
{
#if (DEBUG_LEVEL1)
std::cout << "begin CCalcuvpPose::PoseLowe(...) " << std::endl;
#endif
int n, m; /* nombre d'elements dans la matrice jac */
int lwa; /* taille du vecteur wa */
int ldfjac; /* taille maximum d'une ligne de jac */
int info, ipvt[NBR_PAR];
int tst_lmder;
double f[2 * NBPTMAX], sol[NBR_PAR];
double tol, jac[NBR_PAR][2 * NBPTMAX], wa[2 * NBPTMAX + 50];
// double u[3]; /* vecteur de rotation */
// double rd[3][3]; /* matrice de rotation */
n = NBR_PAR; /* nombres d'inconnues */
m = (int)(2 * npt); /* nombres d'equations */
lwa = 2 * NBPTMAX + 50; /* taille du vecteur de travail */
ldfjac = 2 * NBPTMAX; /* nombre d'elements max sur une ligne */
tol = std::numeric_limits<double>::epsilon(); /* critere d'arret */
// c = cam ;
// for (i=0;i<3;i++)
// for (j=0;j<3;j++) rd[i][j] = cMo[i][j];
// mat_rot(rd,u);
vpRotationMatrix cRo ;
cMo.extract(cRo) ;
vpThetaUVector u(cRo) ;
for (unsigned int i=0;i<3;i++)
{
sol[i] = cMo[i][3];
sol[i+3] = u[i];
}
vpPoint P ;
unsigned int i_=0;
for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
{
P = *it;
XI[i_] = P.get_x();//*cam.px + cam.xc ;
YI[i_] = P.get_y() ;//;*cam.py + cam.yc ;
XO[i_] = P.get_oX();
YO[i_] = P.get_oY();
ZO[i_] = P.get_oZ();
++i_;
}
tst_lmder = lmder1 (&fcn, m, n, sol, f, &jac[0][0], ldfjac, tol, &info, ipvt, lwa, wa);
if (tst_lmder == -1)
{
std::cout << " in CCalculPose::PoseLowe(...) : " ;
std::cout << "pb de minimisation, returns FATAL_ERROR";
// return FATAL_ERROR ;
}
for (unsigned int i = 0; i < 3; i++)
u[i] = sol[i + 3];
for (unsigned int i=0;i<3;i++)
{
cMo[i][3] = sol[i];
u[i] = sol[i+3];
}
vpRotationMatrix rd(u) ;
cMo.insert(rd) ;
// rot_mat(u,rd);
// for (i=0;i<3;i++) for (j=0;j<3;j++) cMo[i][j] = rd[i][j];
#if (DEBUG_LEVEL1)
std::cout << "end CCalculPose::PoseLowe(...) " << std::endl;
#endif
// return OK ;
}