void PoseGraph::optimizeNodes( int iterations )
{
// perform the graph optimization
optimizer->hogman->optimize( iterations, false );
// write the poses back to the environment
// for now, write all the poses back, could add an updated flag
for( std::map<std::string, SensorMaps*>::iterator it = nodeMap.begin();
it != nodeMap.end(); it++ )
{
SensorMaps *sm = it->second;
AISNavigation::PoseGraph3D::Vertex
*vertex = optimizer->hogman->vertex( sm->vertexId );
// get pose with uncertainty from Hogman
envire::TransformWithUncertainty tu(
hogman2Eigen( vertex->transformation ),
hogmanCov2EnvireCov( vertex->covariance ) );
envire::FrameNode::Ptr fn = sm->frameNode;
fn->setTransform( tu );
// update the bounds
// TODO this could be optimized, as it may be to expensive to
// update the bounds everytime we have a small change in position
sm->update();
}
}
static int wj_req_with_res(ost::URLStream &s, const char *url, std::ostream &os)
{
TimeUsed tu(url);
ost::URLStream::Error err = s.get(url);
if (err)
return err;
std::stringstream ss;
while (!s.eof()) {
std::string x;
s >> x;
os << x;
ss << x;
}
if (tu.is_timeout()) {
fprintf(stderr, "\t:%s\n", ss.str().c_str());
}
// 根据 { "result":"ok", } 进行判断.
util_TypedObject to = util_build_object_from_json_string(ss.str().c_str());
if (to.type == 4 && to.data.ov["result"].type == 2 && to.data.ov["result"].data.sv == "ok") {
return 0;
}
else {
fprintf(stderr, "<libptz> ERROR: 云台失败:'%s', str=%s\n", url, ss.str().c_str());
return -1;
}
}
void computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &corners,
const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
{
vpPose pose;
double x=0, y=0;
for (unsigned int i=0; i < point.size(); i ++) {
vpPixelMeterConversion::convertPoint(cam, corners[i], x, y);
point[i].set_x(x);
point[i].set_y(y);
pose.addPoint(point[i]);
}
if (init == true) pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
else pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
vpTranslationVector t;
cMo.extract(t);
vpRotationMatrix R;
cMo.extract(R);
vpThetaUVector tu(R);
std::cout << "cMo: ";
for (unsigned int i=0; i < 3; i++)
std::cout << t[i] << " ";
for (unsigned int i=0; i < 3; i++)
std::cout << vpMath::deg(tu[i]) << " ";
std::cout << std::endl;
}
/*!
Build a 3D \f$ \theta u \f$ visual feature from an
homogeneous matrix \f$ M \f$ that represent the displacement that
the camera has to achieve.
\param M [in] : Homogeneous transformation that describe the
movement that the camera has to achieve. Only the rotational part of
this homogeneous transformation is taken into consideration.
Depending on the rotation representation type
(vpFeatureThetaU::vpFeatureThetaURotationRepresentationType) used to
construct this object, the parameter \e M represents either the
rotation that the camera has to achieve to move from the desired
camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
that the camera has to achieve to move from the current camera frame
to the desired one (\f$ ^{c}R_{c^*}\f$).
*/
void
vpFeatureThetaU::buildFrom(const vpHomogeneousMatrix &M)
{
vpRotationMatrix R ;
M.extract(R) ;
vpThetaUVector tu(R) ;
buildFrom(tu) ;
}
/*!
Initialize a force/torque twist transformation matrix from a translation vector
\f${\bf t}=(t_x, t_y, t_z)^T\f$ and a rotation vector with \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T \f$ parametrization.
\param tx,ty,tz : Translation vector in meters.
\param tux,tuy,tuz : \f$\theta {\bf u}\f$ rotation vector expressed in radians.
*/
vpForceTwistMatrix::vpForceTwistMatrix(const double tx, const double ty, const double tz,
const double tux, const double tuy, const double tuz)
: vpArray2D<double>(6,6)
{
vpTranslationVector T(tx,ty,tz) ;
vpThetaUVector tu(tux,tuy,tuz) ;
buildFrom(T,tu) ;
}
void Helpers::GetCurrentUserAndDomain(std::wstring& strUser, std::wstring& strDomain)
{
std::unique_ptr<void, CloseHandleHelper> hToken(nullptr);
{
HANDLE _hToken = nullptr;
if ( !::OpenProcessToken(
::GetCurrentProcess(),
TOKEN_QUERY,
&_hToken) )
{
Win32Exception::ThrowFromLastError("OpenProcessToken");
}
hToken.reset(_hToken);
}
DWORD dwReturnLength = 0;
if ( !::GetTokenInformation(
hToken.get(),
TokenUser,
nullptr,
0,
&dwReturnLength ) )
{
if( GetLastError() != ERROR_INSUFFICIENT_BUFFER )
Win32Exception::ThrowFromLastError("GetTokenInformation");
}
std::unique_ptr<BYTE[]> tu(new BYTE[dwReturnLength]);
if ( !::GetTokenInformation(
hToken.get(),
TokenUser,
tu.get(),
dwReturnLength,
&dwReturnLength ) )
{
Win32Exception::ThrowFromLastError("GetTokenInformation");
}
wchar_t szUser[CRED_MAX_STRING_LENGTH + 1] = L"";
wchar_t szDomain[CRED_MAX_STRING_LENGTH + 1] = L"";
DWORD dwUserLen = static_cast<DWORD>(ARRAYSIZE(szUser));
DWORD dwDomainLen = static_cast<DWORD>(ARRAYSIZE(szDomain));
SID_NAME_USE snu;
// Retrieve user name and domain name based on user's SID.
if( !LookupAccountSid(NULL, reinterpret_cast<TOKEN_USER*>(tu.get())->User.Sid, szUser, &dwUserLen, szDomain, &dwDomainLen, &snu) )
{
Win32Exception::ThrowFromLastError("LookupAccountSid");
}
strUser = szUser;
strDomain = szDomain;
}
vpFeatureThetaU::vpFeatureThetaU(vpRotationMatrix &R,
vpFeatureThetaURotationRepresentationType r)
: rotation(r)
{
init() ;
vpThetaUVector tu(R) ;
buildFrom(tu) ;
}
/*!
Constructor that build a 3D \f$ \theta u \f$ visual feature from an
homogeneous matrix \f$ M \f$ that represent the displacement that
the camera has to achieve.
\param M [in] : Homogeneous transformation that describe the
movement that the camera has to achieve. Only the rotational part of
this homogeneous transformation is taken into
consideration. Depending on r the rotation represent either the
rotation that the camera has to achieve to move from the desired
camera frame to the current one (\f$ ^{c^*}R_c\f$), or the rotation
that the camera has to achieve to move from the current camera frame
to the desired one (\f$ ^{c}R_{c^*}\f$).
\param r [in] : The rotation representation of M.
*/
vpFeatureThetaU::vpFeatureThetaU(vpHomogeneousMatrix &M,
vpFeatureThetaURotationRepresentationType r)
: rotation(r)
{
init() ;
vpRotationMatrix R ;
M.extract(R) ;
vpThetaUVector tu(R) ;
buildFrom(tu) ;
}
void
vpHomogeneousMatrix::buildFrom(const vpPoseVector &p)
{
vpTranslationVector t(p[0],p[1],p[2]) ;
vpThetaUVector tu(p[3],p[4],p[5]) ;
insert(tu) ;
insert(t) ;
}
/*!
Constructs a quaternion from a rotation matrix.
\param R : Rotation matrix.
*/
void vpQuaternionVector::buildFrom(const vpRotationMatrix &R)
{
vpThetaUVector tu(R);
vpColVector u;
double theta;
tu.extract(theta, u);
theta *= 0.5;
double sinTheta_2 = sin(theta);
set( cos(theta), u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2);
}
/*!
Initialize a force/torque twist transformation matrix from a translation vector
\e t and a rotation vector with \f$\theta u \f$ parametrization.
\param tx,ty,tz : Translation vector in meters.
\param tux,tuy,tuz : \f$\theta u\f$ rotation vector expressed in radians.
*/
vpForceTwistMatrix::vpForceTwistMatrix(const double tx,
const double ty,
const double tz,
const double tux,
const double tuy,
const double tuz) : vpMatrix()
{
init() ;
vpTranslationVector T(tx,ty,tz) ;
vpThetaUVector tu(tux,tuy,tuz) ;
buildFrom(T,tu) ;
}
int MGSBRep::reduce( //BSUDEC
int is_u, //if true, reduce b-rep dimension of u-direction.
int ndec) //Number of B-rep dimension to decrease
//Change the B-Rep by decreasing B-Rep dimension by ndec. This is
//an approximation of the original B-Rep. Return value is error flag.
{
assert(ndec>=0);
if(is_u) assert(bdim_u()-ndec>=order_u());
else assert(bdim_v()-ndec>=order_v());
if(ndec<=0) return 0;
size_t kdec=2; if(is_u) kdec=1;
unsigned ku=order_u(); size_t lud=bdim_u();
unsigned kv=order_v(); size_t lvd=bdim_v();
unsigned maxlen=lud; if(maxlen<lvd) maxlen=lvd;
size_t is1,is2; surface_bcoef().capacity(is1,is2);
int lud2, lvd2;
MGKnotVector tu(ku,lud), tv(kv,lvd);
MGSPointSeq bcoef(lud,lvd,3);
int iflag;
unsigned k=ku; if(k<kv) k=kv;
double* work1=new double[maxlen*(2*k-1)];
double* work2=new double[maxlen];
double* work3=new double[lvd*2];
bsudec_(
ku,lud,knot_data_u(),kv,lvd,knot_data_v(),coef_data(),
kdec,ndec,is1,is2,lud,lvd,work1,work2,work3,
&lud2,&tu(0),&lvd2,&tv(0),&bcoef(0,0,0),&iflag);
if(iflag==1){
tu.set_bdim(lud2); tv.set_bdim(lvd2);
m_uknot=tu; m_vknot=tv;
bcoef.set_length(lud2,lvd2);
m_surface_bcoef=bcoef;
iflag=0;
}
delete[] work1; delete[] work2; delete[] work3;
update_mark();
return iflag;
}
void Doom3TextureManager::initContext(GLContextData& contextData) const
{
/* Create a context data item and store it in the OpenGL context: */
DataItem* dataItem=new DataItem(numTextures);
contextData.addDataItem(this,dataItem);
/* Upload all textures: */
TextureUploader tu(*this,dataItem);
imageTree.forEachLeaf(tu);
/* Unbind all texture objects: */
glBindTexture(GL_TEXTURE_2D,0);
}
// Construct Surface B-rep of order 4 by interpolation from Point data
//and end condition.
// Inner points may include derivative inf.
MGSBRep::MGSBRep(
MGSBRepEndC& endc, //end condition
const MGNDDArray& utaui, //Data point abscissa of u-direction
const MGNDDArray& vtaui, //Data point abscissa of v-direction
const MGSPointSeq& value, //Data point ordinate
int &error) //Error flag.
:MGSurface(){
assert(utaui.length()==value.length_u()
&& vtaui.length()==value.length_v());
MGNDDArray utau,vtau;
add_data_point(endc,utaui,vtaui,utau,vtau);
MGKnotVector tu(utau, 4), tv(vtau,4);
*this=MGSBRep(endc, utaui, vtaui, value, tu, tv, error);
}
void printPose(const std::string &text, const vpHomogeneousMatrix &cMo)
{
vpTranslationVector t;
cMo.extract(t);
vpRotationMatrix R;
cMo.extract(R);
vpThetaUVector tu(R);
std::cout << text;
for (unsigned int i=0; i < 3; i++)
std::cout << t[i] << " ";
for (unsigned int i=0; i < 3; i++)
std::cout << vpMath::deg(tu[i]) << " ";
std::cout << std::endl;
}
// Construct Surface B-rep of specified order by interpolation from Point data.
//This is an easy-to-version.
MGSBRep::MGSBRep( //Derivative Inf.
const MGNDDArray& utau, //Data point of u-direction of value
const MGNDDArray& vtau, //Data point of v-direction of value
const MGSPointSeq& value, //Data point ordinate
size_t orderu, size_t orderv//order along u or v direction.
):MGSurface(){
assert(utau.length()==value.length_u()
&& vtau.length()==value.length_v());
if(utau.length()<orderu) orderu=utau.length();
if(vtau.length()<orderv) orderv=vtau.length();
MGKnotVector tu(utau, orderu), tv(vtau,orderv);
MGSBRepEndC endc;
int error;
*this=MGSBRep(endc, utau, vtau, value, tu, tv, error);
}
int main(int argc, char *argv[])
{
auto index = clang_createIndex(0, 0);
std::vector<std::string> default_args = { {"-x"}, {"c++"}, {"-std=c++11"} };
std::string filename;
ArgList arglist( default_args );
if ( argc > 1 ) {
arglist = ArgList( argc - 2, argv + 1 );
filename = argv[argc - 1];
} else {
filename = argv[1];
}
TUnit tu( index, filename );
if ( !tu.parse( arglist.count(), arglist ) ) {
std::cout << "Translation Unit Initial Parse Failed!\n";
}
std::string input;
std::vector<char> filebuffer;
while( std::getline( std::cin, input ) ) {
if ( input == "REPARSE" ) {
filebuffer = ReparseSource();
CXUnsavedFile unsaved_file = { filename.c_str(),
filebuffer.data(),
filebuffer.size() };
// std::cout << "Size = " << filebuffer.size()
// << "Contents:\n" << filebuffer.data()
// << "\n";
if ( tu.parse( std::vector<CXUnsavedFile>( 1, unsaved_file ) ) ) {
TokenizeSource( tu.handle() );
} else {
std::cout << "Reparse FAILED!\n" << end_pattern << "\n";
}
}
}
clang_disposeIndex( index );
return 0;
}
/*****************************************************************************
* Convert a filename to DOS format.
*
* Input: OutName - Source *and* destination buffer.
*
* NOTE that OutName must point to a memory space that
* is at least 13 bytes in size! That should always be
* the case of course.
*
* ****************************************************************************
*/
void name_mangle(char *OutName)
{
char *p;
p = xstrcpy(OutName);
/*
* check if it's already in 8.3 format
*/
if (!is_8_3(OutName)) {
mangle_name_83(OutName);
} else {
/*
* No mangling needed, convert to uppercase
*/
tu(OutName);
}
free(p);
}
// Construct Surface B-rep of specified order by interpolation from Point data
//and end condition.
//Inner points must not include first derivative inf if the corresponding data
//points are multiple if the order is not 4.
MGSBRep::MGSBRep( //Derivative Inf.
MGSBRepEndC& endc, //end condition
size_t orderu, size_t orderv,//order along u or v direction, must be greater than 4.
const MGNDDArray& utaui, //Data point of u-direction of value
const MGNDDArray& vtaui, //Data point of v-direction of value
const MGSPointSeq& value, //Data point ordinate
int &error) //Error flag.
:MGSurface(){
assert(utaui.length()==value.length_u()
&& vtaui.length()==value.length_v());
assert(orderu>=4 && orderv>=4);
MGNDDArray utau,vtau;
add_data_point(endc,utaui,vtaui,utau,vtau);
size_t nu=utau.length(), nv=vtau.length();
if(orderu>nu) orderu=nu;
if(orderv>nv) orderv=nv;
MGKnotVector tu(utau, orderu), tv(vtau,orderv);
*this=MGSBRep(endc, utaui, vtaui, value, tu, tv, error);
}
/* ************************************************************************** **
* Return True if a name is a special msdos reserved name.
*
* Input: fname - String containing the name to be tested.
*
* Output: True, if the name matches one of the list of reserved names.
*
* Notes: This is a static function called by is_8_3(), below.
*
* ************************************************************************** **
*/
static int is_reserved_msdos( char *fname )
{
char upperFname[13];
char *p;
strncpy (upperFname, fname, 12);
/* lpt1.txt and con.txt etc are also illegal */
p = strchr(upperFname,'.');
if (p)
*p = '\0';
tu(upperFname);
p = upperFname + 1;
switch (upperFname[0]) {
case 'A':
if( 0 == strcmp( p, "UX" ) )
return TRUE;
break;
case 'C':
if ((0 == strcmp( p, "LOCK$" )) || (0 == strcmp( p, "ON" )) || (0 == strcmp( p, "OM1" ))
|| (0 == strcmp( p, "OM2" )) || (0 == strcmp( p, "OM3" )) || (0 == strcmp( p, "OM4" )))
return TRUE;
break;
case 'L':
if( (0 == strcmp( p, "PT1" )) || (0 == strcmp( p, "PT2" )) || (0 == strcmp( p, "PT3" )))
return TRUE;
break;
case 'N':
if( 0 == strcmp( p, "UL" ) )
return TRUE;
break;
case 'P':
if( 0 == strcmp( p, "RN" ) )
return TRUE;
break;
}
return FALSE;
} /* is_reserved_msdos */
geometry_msgs::Pose toGeometryMsgsPose(const vpHomogeneousMatrix& mat){
geometry_msgs::Pose pose;
vpThetaUVector tu(mat);
vpColVector u;
double theta;
tu.extract(theta, u);
theta *= 0.5;
double sinTheta_2 = sin(theta);
pose.orientation.x = u[0] * sinTheta_2;
pose.orientation.y = u[1] * sinTheta_2;
pose.orientation.z = u[2] * sinTheta_2;
pose.orientation.w = cos(theta);
pose.position.x = mat[0][3];
pose.position.y = mat[1][3];
pose.position.z = mat[2][3];
return pose;
}
int main(int argc, char **argv)
{
(void)argc;
(void)argv;
Bureaucrat tu("yolo", 8);
bool tutu = true;
while (tutu)
{
try
{
tu.incrementGrade();
std::cout << tu << std::endl;
}
catch (std::exception & e)
{
tutu = false;
std::cout << "Exception " << e.what() << '\n';
}
}
while (!tutu)
{
try
{
tu.decrementGrade();
std::cout << tu << std::endl;
}
catch (std::exception & e)
{
tutu = true;
std::cout << "Exception " << e.what() << '\n';
}
}
return (0);
}
bool JsonIRReader::ReadDump(const std::string &dump_file) {
Json::Value tu_json;
Json::Reader reader;
std::ifstream input(dump_file);
if (!reader.parse(input, tu_json, /* collectComments */ false)) {
llvm::errs() << "Failed to parse JSON: "
<< reader.getFormattedErrorMessages() << "\n";
return false;
}
bool ok = true;
JsonObjectRef tu(tu_json, ok);
if (!ok) {
llvm::errs() << "Translation unit is not an object\n";
return false;
}
ReadFunctions(tu);
ReadGlobalVariables(tu);
ReadEnumTypes(tu);
ReadRecordTypes(tu);
ReadFunctionTypes(tu);
ReadArrayTypes(tu);
ReadPointerTypes(tu);
ReadQualifiedTypes(tu);
ReadBuiltinTypes(tu);
ReadLvalueReferenceTypes(tu);
ReadRvalueReferenceTypes(tu);
ReadElfFunctions(tu);
ReadElfObjects(tu);
if (!ok) {
llvm::errs() << "Failed to convert JSON to IR\n";
return false;
}
return true;
}
int
main()
{
try
{
vpImage<unsigned char> I ;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
g.open(I) ;
g.acquire(I) ;
vpDisplayX display(I,100,100,"testDisplayX.cpp ") ;
vpTRACE(" ") ;
vpDisplay::display(I) ;
vpDisplay::flush(I);
vpServo task ;
vpRobotAfma6 robot ;
//robot.move("zero.pos") ;
vpCameraParameters cam ;
// Update camera parameters
robot.getCameraParameters (cam, I);
std::cout << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << " Test program for vpServo " <<std::endl ;
std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
std::cout << " Simulation " << std::endl ;
std::cout << " task : servo a line " << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << std::endl ;
int nbline =4 ;
int nbpoint =4 ;
vpTRACE("sets the desired position of the visual feature ") ;
vpPoint pointd[nbpoint]; //position of the fours corners
vpPoint pointcd; //position of the center of the square
vpFeaturePoint pd;
double L=0.05 ;
pointd[0].setWorldCoordinates(L,-L, 0 ) ;
pointd[1].setWorldCoordinates(L,L, 0 ) ;
pointd[2].setWorldCoordinates(-L,L, 0 ) ;
pointd[3].setWorldCoordinates(-L,-L, 0 ) ;
//The coordinates in the object frame of the point used as a feature ie the center of the square
pointcd.setWorldCoordinates(0, 0, 0 ) ;
//The desired homogeneous matrix.
vpHomogeneousMatrix cMod(0,0,0.4,0,0,vpMath::rad(10));
pointd[0].project(cMod);
pointd[1].project(cMod);
pointd[2].project(cMod);
pointd[3].project(cMod);
pointcd.project(cMod);
vpFeatureBuilder::create(pd,pointcd);
vpTRACE("Initialization of the tracking") ;
vpMeLine line[nbline] ;
vpPoint point[nbpoint];
int i ;
vpMe me ;
me.setRange(10) ;
me.setPointsToTrack(100) ;
me.setThreshold(50000) ;
me.setSampleStep(10);
//Initialize the tracking. Define the four lines to track
for (i=0 ; i < nbline ; i++)
{
line[i].setMe(&me) ;
line[i].initTracking(I) ;
line[i].track(I) ;
}
// Compute the position of the four corners. The goal is to
// compute the pose
vpImagePoint ip;
for (i=0 ; i < nbline ; i++)
{
double x=0, y=0;
if (!vpMeLine::intersection (line[i%nbline], line[(i+1)%nbline], ip))
{
exit(-1);
}
vpPixelMeterConversion::convertPoint(cam, ip, x, y) ;
point[i].set_x(x) ;
point[i].set_y(y) ;
}
//Compute the pose cMo
vpPose pose ;
pose.clearPoint() ;
vpHomogeneousMatrix cMo ;
point[0].setWorldCoordinates(L,-L, 0 ) ;
point[1].setWorldCoordinates(L,L, 0 ) ;
point[2].setWorldCoordinates(-L,L, 0 ) ;
point[3].setWorldCoordinates(-L,-L, 0 ) ;
for (i=0 ; i < nbline ; i++)
{
pose.addPoint(point[i]) ; // and added to the pose computation point list
}
pose.computePose(vpPose::LAGRANGE, cMo) ;
pose.computePose(vpPose::VIRTUAL_VS, cMo);
vpTRACE("sets the current position of the visual feature ") ;
//The first features are the position in the camera frame x and y of the square center
vpPoint pointc; //The current position of the center of the square
double xc = (point[0].get_x()+point[2].get_x())/2;
double yc = (point[0].get_y()+point[2].get_y())/2;
pointc.set_x(xc);
pointc.set_y(yc);
vpFeaturePoint p;
pointc.project(cMo);
vpFeatureBuilder::create(p,pointc);
//The second feature is the depth of the current square center relative to the depth of the desired square center.
vpFeatureDepth logZ;
logZ.buildFrom(pointc.get_x(), pointc.get_y(), pointc.get_Z(), log(pointc.get_Z()/pointcd.get_Z()));
//The last three features are the rotations thetau between the current pose and the desired pose.
vpHomogeneousMatrix cdMc ;
cdMc = cMod*cMo.inverse() ;
vpFeatureThetaU tu(vpFeatureThetaU::cdRc);
tu.buildFrom(cdMc) ;
vpTRACE("define the task") ;
vpTRACE("\t we want an eye-in-hand control law") ;
vpTRACE("\t robot is controlled in the camera frame") ;
task.setServo(vpServo::EYEINHAND_CAMERA) ;
task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
vpTRACE("\t we want to see a point on a point..") ;
std::cout << std::endl ;
task.addFeature(p,pd) ;
task.addFeature(logZ) ;
task.addFeature(tu);
vpTRACE("\t set the gain") ;
task.setLambda(0.2) ;
vpTRACE("Display task information " ) ;
task.print() ;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
int iter=0 ;
vpTRACE("\t loop") ;
vpColVector v ;
vpImage<vpRGBa> Ic ;
double lambda_av =0.05;
double alpha = 0.05 ;
double beta =3 ;
double erreur= 1;
while(1)
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
try {
g.acquire(I) ;
vpDisplay::display(I) ;
pose.clearPoint() ;
//Track the lines and find the current position of the corners
for (i=0 ; i < nbline ; i++)
{
line[i].track(I) ;
line[i].display(I,vpColor::green);
double x=0, y=0;
if (!vpMeLine::intersection (line[i%nbline], line[(i+1)%nbline], ip))
{
exit(-1);
}
vpPixelMeterConversion::convertPoint(cam, ip, x, y) ;
point[i].set_x(x);
point[i].set_y(y);
pose.addPoint(point[i]) ;
}
//Compute the pose
pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
//Update the two first features x and y (position of the square center)
xc = (point[0].get_x()+point[2].get_x())/2;
yc = (point[0].get_y()+point[2].get_y())/2;
pointc.set_x(xc);
pointc.set_y(yc);
pointc.project(cMo);
vpFeatureBuilder::create(p,pointc);
//Print the current and the desired position of the center of the square
//Print the desired position of the four corners
p.display(cam, I, vpColor::green) ;
pd.display(cam, I, vpColor::red) ;
for (i = 0; i < nbpoint; i++) pointd[i].display(I, cam, vpColor::red);
//Update the second feature
logZ.buildFrom(pointc.get_x(), pointc.get_y(), pointc.get_Z(), log(pointc.get_Z()/pointcd.get_Z()));
//Update the last three features
cdMc = cMod*cMo.inverse() ;
tu.buildFrom(cdMc) ;
//Adaptive gain
double gain ;
{
if (alpha == 0) gain = lambda_av ;
else
{
gain = alpha * exp (-beta * task.error.sumSquare() ) + lambda_av ;
}
}
task.setLambda(gain) ;
v = task.computeControlLaw() ;
vpDisplay::flush(I) ;
std::cout << v.sumSquare() <<std::endl ;
if (iter==0) vpDisplay::getClick(I) ;
if (v.sumSquare() > 0.5)
{
v =0 ;
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
robot.stopMotion() ;
vpDisplay::getClick(I) ;
}
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
}
catch(...)
{
v =0 ;
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
robot.stopMotion() ;
exit(1) ;
}
vpTRACE("\t\t || s - s* || = %f ", task.error.sumSquare()) ;
erreur = task.error.sumSquare();
iter++;
}
vpTRACE("Display task information " ) ;
task.print() ;
task.kill();
}
catch (...)
{
vpERROR_TRACE(" Test failed") ;
return 0;
}
}
/*****************************************************************************
* do the actual mangling to 8.3 format
* the buffer must be able to hold 13 characters (including the null)
*****************************************************************************
*/
void mangle_name_83(char *s)
{
int crc16, i;
char *p, *q;
char extension[4];
char base[9];
int baselen = 0;
int extlen = 0;
extension[0] = 0;
base[0] = 0;
/*
* First, convert some common Unix extensions to extensions of 3
* characters. If none fits, don't change anything now.
*/
if (strcmp(q = s + strlen(s) - strlen(".tar.gz"), ".tar.gz") == 0) {
*q = '\0';
q = (char *)"tgz";
} else if (strcmp(q = s + strlen(s) - strlen(".tar.z"), ".tar.z") == 0) {
*q = '\0';
q = (char *)"tgz";
} else if (strcmp(q = s + strlen(s) - strlen(".tar.Z"), ".tar.Z") == 0) {
*q = '\0';
q = (char *)"taz";
} else if (strcmp(q = s + strlen(s) - strlen(".html"), ".html") == 0) {
*q = '\0';
q = (char *)"htm";
} else if (strcmp(q = s + strlen(s) - strlen(".shtml"), ".shtml") == 0) {
*q = '\0';
q = (char *)"stm";
} else if (strcmp(q = s + strlen(s) - strlen(".conf"), ".conf") == 0) {
*q = '\0';
q = (char *)"cnf";
} else if (strcmp(q = s + strlen(s) - strlen(".mpeg"), ".mpeg") == 0) {
*q = '\0';
q = (char *)"mpg";
} else if (strcmp(q = s + strlen(s) - strlen(".smil"), ".smil") == 0) {
*q = '\0';
q = (char *)"smi";
} else if (strcmp(q = s + strlen(s) - strlen(".perl"), ".perl") == 0) {
*q = '\0';
q = (char *)"pl";
} else if (strcmp(q = s + strlen(s) - strlen(".jpeg"), ".jpeg") == 0) {
*q = '\0';
q = (char *)"jpg";
} else if (strcmp(q = s + strlen(s) - strlen(".tiff"), ".tiff") == 0) {
*q = '\0';
q = (char *)"tif";
} else {
q = NULL;
}
if (q) {
/*
* Extension is modified, apply changes
*/
p = s + strlen(s);
*p++ = '.';
for (i = 0; i < strlen(q); i++)
*p++ = q[i];
*p++ = '\0';
}
/*
* Now start name mangling
*/
p = strrchr(s,'.');
if (p && (strlen(p+1) < (size_t)4)) {
int all_normal = (!strhaslower(p+1)); /* XXXXXXXXX */
if (all_normal && p[1] != 0) {
*p = 0;
crc16 = crc16xmodem(s, strlen(s));
*p = '.';
} else {
crc16 = crc16xmodem(s, strlen(s));
}
} else {
crc16 = crc16xmodem(s, strlen(s));
}
tu(s);
if (p) {
if (p == s)
safe_strcpy(extension, "___", 3);
else {
*p++ = 0;
while (*p && extlen < 3) {
if (*p != '.' )
extension[extlen++] = p[0];
p++;
}
extension[extlen] = 0;
}
}
p = s;
/*
* Changed to baselen 4, original this is 5.
* 24-11-2002 MB.
*/
while (*p && baselen < 4) {
if (*p != '.' )
base[baselen++] = p[0];
p++;
}
base[baselen] = 0;
if (crc16 > (MANGLE_BASE * MANGLE_BASE * MANGLE_BASE))
Syslog('!', "WARNING: mangle_name_83() crc16 overflow");
crc16 = crc16 % (MANGLE_BASE * MANGLE_BASE * MANGLE_BASE);
snprintf(s, 9, "%s%c%c%c%c", base, magic_char,
mangle(crc16 / (MANGLE_BASE * MANGLE_BASE)), mangle(crc16 / MANGLE_BASE), mangle(crc16));
if ( *extension ) {
(void)strcat(s, ".");
(void)strcat(s, extension);
}
}
void RegularBattleScene::updateToolTip(int spot)
{
QString tooltip;
QString stats[7] = {
tu(StatInfo::Stat(1, data()->gen())),
tu(StatInfo::Stat(2, data()->gen())),
tu(StatInfo::Stat(3, data()->gen())),
tu(StatInfo::Stat(4, data()->gen())),
tu(StatInfo::Stat(5, data()->gen())),
tu(StatInfo::Stat(6, data()->gen())),
tu(StatInfo::Stat(7, data()->gen()))
};
/* Putting dots after stat names so the ":" is always at the same place */
int max = 0;
for (int i = 0; i < 7; i++) {
max = std::max(max, stats[i].length());
}
for (int i = 0; i < 7; i++) {
stats[i] = stats[i].leftJustified(max, '.', false);
}
const auto &poke = data()->poke(spot);
tooltip += nick(spot) + "\n";
tooltip += TypeInfo::Name(PokemonInfo::Type1(poke.num(), data()->gen()));
int type2 = PokemonInfo::Type2(poke.num(), data()->gen());
if (type2 != Pokemon::Curse) {
tooltip += " " + TypeInfo::Name(PokemonInfo::Type2(poke.num(), data()->gen()));
}
tooltip += "\n";
for (int i = 0; i < 5; i++) {
// Gen 1 only has Special, and we treat SAtk as Special hiding SDef.
if (data()->gen().num == 1) {
switch (i) {
case 3: continue;
default: tooltip += "\n" + stats[i] + " ";
}
} else {
tooltip += "\n" + stats[i] + " ";
}
int boost = data()->fieldPoke(spot).statBoost(i+1);
int stat = data()->fieldPoke(spot).stat(i+1);
if (stat == 0) {
if (boost >= 0) {
tooltip += QString("+%1").arg(boost);
} else if (boost < 0) {
tooltip += QString("%1").arg(boost);
}
} else {
if (stat == -1) {
tooltip += "???";
} else {
tooltip += QString::number(stat);
}
if (boost >= 0) {
tooltip += QString("(+%1)").arg(boost);
} else if (boost < 0) {
tooltip += QString("(%1)").arg(boost);
}
}
}
for (int i = 5; i < 7; i++) {
int boost = data()->fieldPoke(spot).statBoost(i+1);
if (boost) {
tooltip += "\n" + stats[i] + " ";
if (boost > 0) {
tooltip += QString("+%1").arg(boost);
} else if (boost < 0) {
tooltip += QString("%1").arg(boost);
}
}
}
tooltip += "\n";
const auto &zone = *data()->field().zone(data()->player(spot));
if (zone.spikesLevel() > 0) {
tooltip += "\n" + tr("Spikes level %1").arg(zone.spikesLevel());
}
if (zone.tspikesLevel() > 0) {
tooltip += "\n" + tr("Toxic Spikes level %1").arg(zone.tspikesLevel());
}
if (zone.stealthRocks()) {
tooltip += "\n" + tr("Stealth Rock");
}
if (data()->field().weather() != Weather::NormalWeather) {
tooltip += "\n" + tr("Weather: %1").arg(TypeInfo::weatherName(data()->field().weather()));
}
gui.zone->tooltips[spot] = tooltip;
}
BOOL SkeletalTracker::SkeletalTrackingProc()
{
//Util tu("算法耗时");
const int frameWidth = 320;
const int frameHeight = 240;
const USHORT * pDepthFrame = m_pNuiImageDevice->GetDepthMap();;
const USHORT * pBufferRun = pDepthFrame;
const USHORT * pBufferEnd = pBufferRun + (320 * 240);
// STEP2: 检查微软深度图有无PlayerIndex, 最终应该是BGR模块检测
BOOL bHavePlayer = FALSE;
pBufferRun = pDepthFrame;
while ( pBufferRun < pBufferEnd )
{
USHORT depth = *pBufferRun;
if(depth & 0x0007)
{
bHavePlayer = TRUE;
break;
}
++pBufferRun;
}
// 耗时估计:
// STEP1之前: 12% ~ 13%
// STEP3之前: 12% ~ 13%
// STEP4之前: 23% ~ 25%
// STEP5之前: 66% ~ 70%, STEP4消耗资源太多
if (bHavePlayer)
{
m_pPipelineSourceStage->RunFrame();
m_pBGRStage->RunFrame();
m_pExemplarStage->RunFrame();
m_pCentroidsStage->RunFrame();
m_pModelFittingStage->RunFrame();
m_pPipelineOutputStage->RunFrame();
#if 0
{
// STEP3: 人体部位分类
MEASURE_TIME(Util tu("人体部位分类"));
m_pBPC.ExemplarStage_RunFrame(1.0f, pDepthFrame, m_pLabelRaw, 1, LABEL_DATA);
}
{
// STEP4: 提取部位中心
MEASURE_TIME(Util tu("提取部位中心"));
m_pBPC.CentroidsStage_RunFrame(pDepthFrame, m_pLabelRaw, m_centroid);
}
{
// STEP5: 提取人体骨骼
MEASURE_TIME(Util tu("提取人体骨骼"));
m_pBPC.ModelFittingStage_RunFrame(m_centroid, m_joints);
}
{
// STEP6: 平滑人体骨骼
MEASURE_TIME(Util tu("平滑人体骨骼"));
NUI_TRANSFORM_SMOOTH_PARAMETERS SmoothingParams;
SmoothingParams.fSmoothing = 0.5f;
SmoothingParams.fCorrection = 0.5f;
SmoothingParams.fPrediction = 0.5f;
SmoothingParams.fJitterRadius = 0.05f;
SmoothingParams.fMaxDeviationRadius = 0.04f;
if (S_OK != m_pBPC.TransformSmooth(m_joints, &SmoothingParams))
{
assert(0);
}
}
#endif
}
return TRUE;
}
void RegularBattleScene::updateToolTip(int spot)
{
QString tooltip;
QString stats[8] = {
tu(StatInfo::Stat(0, data()->gen())),
tu(StatInfo::Stat(1, data()->gen())),
tu(StatInfo::Stat(2, data()->gen())),
tu(StatInfo::Stat(3, data()->gen())),
tu(StatInfo::Stat(4, data()->gen())),
tu(StatInfo::Stat(5, data()->gen())),
tu(StatInfo::Stat(6, data()->gen())),
tu(StatInfo::Stat(7, data()->gen()))
};
//Prevents a headache if it includes HP as a null value
QString range[6] = { 0 };
QString statval[6] = { 0 };
for (int i = 1; i < 6; i++) {
int min = data()->fieldPoke(spot).minStat(i);
int max = data()->fieldPoke(spot).maxStat(i);
range[i] = QString("%2-%3").arg(min).arg(max);
int stat = data()->fieldPoke(spot).stat(i);
if (stat == -1) {
statval[i] = "???";
} else {
statval[i] = QString::number(stat);
}
}
/* Aligning various things on the tool tip */
int max = 0, max2 = 0, max3 = 0;
for (int i = 0; i < 8; i++) {
max = std::max(max, stats[i].length());
if (i > 0 && i < 6) {
max2 = std::max(max2, range[i].length());
max3 = std::max(max3, statval[i].length());
}
}
for (int i = 0; i < 8; i++) {
stats[i] = stats[i].leftJustified(max, '.', false);
if (i > 0 && i < 6) {
range[i] = range[i].leftJustified(max2, ' ', false);
statval[i] = statval[i].rightJustified(max3, ' ', false);
}
}
const auto &poke = data()->poke(spot).num();
tooltip += nick(spot) + "\n";
tooltip += TypeInfo::Name(PokemonInfo::Type1(poke, data()->gen()));
int type2 = PokemonInfo::Type2(poke, data()->gen());
if (type2 != Pokemon::Curse) {
tooltip += " " + TypeInfo::Name(type2);
}
tooltip += "\n";
for (int i = 0; i < 6; i++) {
// Gen 1 only has Special, and we treat SAtk as Special hiding SDef.
if (data()->gen().num == 1) {
switch (i) {
case 4:
continue;
default:
tooltip += "\n" + stats[i] + " ";
}
} else {
tooltip += "\n" + stats[i] + " ";
}
int boost = data()->fieldPoke(spot).statBoost(i);
int stat = data()->fieldPoke(spot).stat(i);
if (stat == 0 || i == 0) {
int min = data()->fieldPoke(spot).minStat(i);
int max = data()->fieldPoke(spot).maxStat(i);
if (i == 0) {
if (data()->isPlayer(spot)) {
tooltip += QString("%1/%2").arg(data()->poke(spot).life()).arg(data()->poke(spot).totalLife());
} else {
tooltip += QString("%3/%1-%4/%2").arg(min).arg(max).arg(gui.bars[spot]->value()*min/100).arg(gui.bars[spot]->value()*max/100);
}
} else {
if (boost >= 0) {
tooltip += range[i] + (boost > 0 ? QString(" (+%1)").arg(boost) : "");
} else if (boost < 0) {
tooltip += range[i] + QString(" (%1)").arg(boost);
}
}
} else {
if (boost >= 0) {
tooltip += statval[i] + (boost > 0 ? QString(" (+%1)").arg(boost) : "");
} else if (boost < 0) {
tooltip += statval[i] + QString(" (%1)").arg(boost);
}
}
}
for (int i = 6; i < 8; i++) {
int boost = data()->fieldPoke(spot).statBoost(i);
if (boost) {
tooltip += "\n" + stats[i] + " ";
if (boost > 0) {
tooltip += QString("+%1").arg(boost);
} else if (boost < 0) {
tooltip += QString("%1").arg(boost);
}
}
}
tooltip += "\n";
const auto &zone = *data()->field().zone(data()->player(spot));
if (zone.spikesLevel() > 0) {
tooltip += "\n" + tr("Spikes level %1").arg(zone.spikesLevel());
}
if (zone.tspikesLevel() > 0) {
tooltip += "\n" + tr("Toxic Spikes level %1").arg(zone.tspikesLevel());
}
if (zone.stealthRocks()) {
tooltip += "\n" + tr("Stealth Rock");
}
if (zone.stickyWeb()) {
tooltip += "\n" + tr("Sticky Web");
}
if (data()->field().weather() != Weather::NormalWeather) {
tooltip += "\n" + tr("Weather: %1").arg(TypeInfo::weatherName(data()->field().weather()));
}
if (!data()->isPlayer(spot) && data()->poke(spot).move(0)->num() != Move::NoMove) {
tooltip += "\n\nKnown Moves:\n";
for (int i = 0; i < 4; i++) {
int move = data()->poke(spot).move(i)->num();
if (move != Move::NoMove) {
tooltip += MoveInfo::Name(move) + QString(" %1/%2").arg(data()->poke(spot).move(i)->PP()).arg(data()->poke(spot).move(i)->totalPP()) + "\n";
} else {
tooltip += "??? \n";
}
}
}
gui.zone->tooltips[spot] = tooltip;
}
void menu()
{
FILE *pMenuFile;
int Key, IsANSI;
char temp[81], *Input, *sMenuPathFileName, buf[81];
Input = calloc(PATH_MAX, sizeof(char));
sMenuPathFileName = calloc(PATH_MAX, sizeof(char));
Syslog('+', "Starting menu loop");
/*
* Loop forever, this is what a BBS should do until a user logs out.
*/
while (TRUE) {
WhosDoingWhat(BROWSING, NULL);
/*
* Open menufile, first users language menu, if it fails
* try to open the default menu.
*/
snprintf(sMenuPathFileName, PATH_MAX, "%s/share/int/menus/%s/%s", getenv("FTND_ROOT"), lang.lc, Menus[MenuLevel]);
if ((pMenuFile = fopen(sMenuPathFileName, "r")) == NULL) {
snprintf(sMenuPathFileName, PATH_MAX, "%s/share/int/menus/%s/%s", getenv("FTND_ROOT"), CFG.deflang, Menus[MenuLevel]);
pMenuFile = fopen(sMenuPathFileName,"r");
if (pMenuFile != NULL)
Syslog('b', "Menu %s (Default)", Menus[MenuLevel]);
} else {
Syslog('b', "Menu %s (%s)", Menus[MenuLevel], lang.Name);
}
if (pMenuFile == NULL) {
clear();
WriteError("Can't open menu file: %s", sMenuPathFileName);
MenuError++;
/*
* Is this the last attempt to open the default menu?
*/
if (MenuError == 10) {
WriteError("FATAL ERROR: Too many menu errors");
snprintf(temp, 81, "Too many menu errors, notifying Sysop\r\n\r\n");
PUTSTR(temp);
sleep(3);
die(FTNERR_CONFIG_ERROR);
}
/*
* Switch back to the default menu
*/
MenuLevel = 0;
strcpy(Menus[0], CFG.default_menu);
} else {
/*
* Display Menu Text Fields and Perform all autoexec menus in order of menu file.
* First check if there are any ANSI menus, if not, send a clearscreen first.
*/
IsANSI = FALSE;
while (fread(&menus, sizeof(menus), 1, pMenuFile) == 1) {
if ( Le_Access(exitinfo.Security, menus.MenuSecurity) && (UserAge >= le_int(menus.Age))){
if ((le_int(menus.MenuType) == 5) || (le_int(menus.MenuType) == 19) || (le_int(menus.MenuType) == 20))
IsANSI = TRUE;
}
}
fseek(pMenuFile, 0, SEEK_SET);
if (! IsANSI)
clear();
while (fread(&menus, sizeof(menus), 1, pMenuFile) == 1) {
if ( Le_Access(exitinfo.Security, menus.MenuSecurity) && (UserAge >= le_int(menus.Age))){
if (menus.AutoExec) {
DoMenu( le_int(menus.MenuType) );
}
DisplayMenu( );
}
}
/*
* Check if the BBS closed down for Zone Mail Hour or
* system shutdown. If so, we run the Goodbye show.
*/
if (CheckStatus() == FALSE) {
fclose(pMenuFile);
Syslog('+', "Kicking user out, the BBS is closed.");
sleep(3);
Good_Bye(FTNERR_OK);
}
/*
* Check the upsdown semafore
*/
if (IsSema((char *)"upsdown")) {
fclose(pMenuFile);
Syslog('+', "Kicking user out, upsdown semafore detected");
snprintf(temp, 81, "System power failure, closing the bbs");
PUTSTR(temp);
Enter(2);
sleep(3);
Good_Bye(FTNERR_OK);
}
/*
* Check if SysOp wants to chat to user everytime user gets prompt.
*/
if (CFG.iChatPromptChk) {
snprintf(buf, 81, "CISC:1,%d", mypid);
if (socket_send(buf) == 0) {
strcpy(buf, socket_receive());
if (strcmp(buf, "100:1,1;") == 0) {
Syslog('+', "Forced sysop/user chat");
Chat(exitinfo.Name, (char *)"#sysop");
continue;
}
}
}
/*
* Check users timeleft
*/
TimeCheck();
alarm_on();
if (exitinfo.HotKeys) {
Key = Readkey();
snprintf(Input, 81, "%c", Key);
Enter(1);
} else {
colour(CFG.InputColourF, CFG.InputColourB);
GetstrC(Input, 80);
}
if ((strcmp(Input, "")) != 0) {
fseek(pMenuFile, 0, SEEK_SET);
while (fread(&menus, sizeof(menus), 1, pMenuFile) == 1) {
if ((strcmp(tu(Input), menus.MenuKey)) == 0) {
if ((Le_Access(exitinfo.Security, menus.MenuSecurity)) && (UserAge >= le_int(menus.Age))) {
Syslog('+', "Menu[%d] %d=(%s), Opt: '%s'", MenuLevel, le_int(menus.MenuType),
menus.TypeDesc, menus.OptionalData);
if (le_int(menus.MenuType) == 13) {
/*
* Terminate call, cleanup here
*/
free(Input);
free(sMenuPathFileName);
fclose(pMenuFile);
}
DoMenu(le_int(menus.MenuType));
break;
}
}
}
}
fclose(pMenuFile);
} /* If menu open */
} /* while true */
}
int
main(int argc, const char ** argv)
{
// Read the command line options
if (getOptions(argc, argv) == false) {
exit (-1);
}
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
// - the 6 computed camera velocities (m/s, rad/s) to achieve the task
// - the 6 values of s - s*
std::string username;
// Get the user login name
vpIoTools::getUserName(username);
// Create a log filename to save velocities...
std::string logdirname;
#ifdef WIN32
logdirname ="C:/temp/" + username;
#else
logdirname ="/tmp/" + username;
#endif
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
}
catch (...) {
std::cerr << std::endl
<< "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
}
std::string logfilename;
logfilename = logdirname + "/log.dat";
// Open the log file name
std::ofstream flog(logfilename.c_str());
vpServo task ;
vpRobotCamera robot ;
std::cout << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << " Test program for vpServo " <<std::endl ;
std::cout << " Eye-in-hand task control, velocity computed in the camera frame" << std::endl ;
std::cout << " Simulation " << std::endl ;
std::cout << " task : 3D visual servoing " << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << std::endl ;
// Sets the initial camera location
vpPoseVector c_r_o(// Translation tx,ty,tz
0.1, 0.2, 2,
// ThetaU rotation
vpMath::rad(20), vpMath::rad(10), vpMath::rad(50) ) ;
// From the camera pose build the corresponding homogeneous matrix
vpHomogeneousMatrix cMo(c_r_o) ;
// Set the robot initial position
robot.setPosition(cMo) ;
// Sets the desired camera location
vpPoseVector cd_r_o(// Translation tx,ty,tz
0, 0, 1,
// ThetaU rotation
vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)) ;
// From the camera desired pose build the corresponding homogeneous matrix
vpHomogeneousMatrix cdMo(cd_r_o) ;
// Compute the homogeneous transformation from the desired camera position to the initial one
vpHomogeneousMatrix cdMc ;
cdMc = cdMo*cMo.inverse() ;
// Build the current visual features s = (c*tc, thetaU_c*Rc)^T
vpFeatureTranslation t(vpFeatureTranslation::cdMc) ;
vpFeatureThetaU tu(vpFeatureThetaU::cdRc); // current feature
t.buildFrom(cdMc) ;
tu.buildFrom(cdMc) ;
// Sets the desired rotation (always zero !) since s is the
// rotation that the camera has to achieve. Here s* = (0, 0)^T
vpFeatureTranslation td(vpFeatureTranslation::cdMc) ;
vpFeatureThetaU tud(vpFeatureThetaU::cdRc); // desired feature
// Define the task
// - we want an eye-in-hand control law
// - the robot is controlled in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA) ;
// - we use here the interaction matrix computed with the
// current features
task.setInteractionMatrixType(vpServo::CURRENT);
// Add the current and desired visual features
task.addFeature(t,td) ; // 3D translation
task.addFeature(tu,tud) ; // 3D rotation
// - set the constant gain to 1.0
task.setLambda(1) ;
// Display task information
task.print() ;
int iter=0 ;
// Start the visual servoing loop. We stop the servo after 200 iterations
while(iter++ < 200) {
std::cout << "-----------------------------------" << iter <<std::endl ;
vpColVector v ;
// get the robot position
robot.getPosition(cMo) ;
// new displacement to achieve
cdMc = cdMo*cMo.inverse() ;
// Update the current visual features
t.buildFrom(cdMc) ;
tu.buildFrom(cdMc) ;
// Compute the control law
v = task.computeControlLaw() ;
// Display task information
if (iter==1) task.print() ;
// Send the camera velocity to the controller
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
// Retrieve the error
std::cout << task.error.sumSquare() <<std::endl ;
// Save log
flog << v.t() << " " << task.error.t() << std::endl;
}
// Display task information
task.print() ;
// Kill the task
task.kill();
// Close the log file
flog.close();
}
