// Coprocessor Data (CDP) Instructions
// type = Shuffle: cp_num=p1, opcode2=111
// Opcode1 = d d c c
void wshufh(WMMXRegister wRd, WMMXRegister wRn, WMMXRegister wRm,
int Imm8, Condition cond = al) {
int bbaa, ddcc;
bbaa = Imm8 & 0xF;
ddcc = (Imm8 & 0xF0) >> 0x4;
cdp(p1, ddcc, (CRegister)wRd, (CRegister)wRn,
(CRegister)bbaa, 0x7, cond);
}
void UpnpCDWidgetFactory::OnService(const std::string& url,
const std::string& udn)
{
import::RemoteCDDrive *cd = new import::RemoteCDDrive(m_client, m_server,
m_poller);
unsigned rc = cd->Init(url, udn);
if (rc != 0)
{
free(cd);
return;
}
import::CDDrivePtr cdp(cd);
(void) new CDWidget(m_parent, cdp, *m_pixmap, m_settings, m_cpu_queue,
m_disk_queue);
}
/*!
Set the pose to be used in entry (as guess) of the next call to the track() function.
This pose will be just used once.
\warning This functionnality is not available when tracking cylinders.
\param I : image corresponding to the desired pose.
\param cdMo : Pose to affect.
*/
void
vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo)
{
if((int)(kltCylinders.size()) != 0)
{
std::cout << "WARNING: Cannot set pose when model contains cylinder(s). This feature is not implemented yet." << std::endl;
std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
cMo = cdMo;
init(I);
}
else
{
vpMbtDistanceKltPoints *kltpoly;
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
std::vector<cv::Point2f> init_pts;
std::vector<long> init_ids;
std::vector<cv::Point2f> guess_pts;
#else
unsigned int nbp = 0;
for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it) {
kltpoly = *it;
if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2)
nbp += (*it)->getCurrentNumberPoints();
}
CvPoint2D32f* init_pts = NULL;
init_pts = (CvPoint2D32f*)cvAlloc(tracker.getMaxFeatures()*sizeof(init_pts[0]));
long *init_ids = (long*)cvAlloc((unsigned int)tracker.getMaxFeatures()*sizeof(long));
unsigned int iter_pts = 0;
CvPoint2D32f* guess_pts = NULL;
guess_pts = (CvPoint2D32f*)cvAlloc(tracker.getMaxFeatures()*sizeof(guess_pts[0]));
#endif
vpHomogeneousMatrix cdMc = cdMo * cMo.inverse();
vpHomogeneousMatrix cMcd = cdMc.inverse();
vpRotationMatrix cdRc;
vpTranslationVector cdtc;
cdMc.extract(cdRc);
cdMc.extract(cdtc);
unsigned int nbCur = 0;
for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it) {
kltpoly = *it;
if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
kltpoly->polygon->changeFrame(cdMo);
//Get the normal to the face at the current state cMo
vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
plan.changeFrame(cMcd);
vpColVector Nc = plan.getNormal();
Nc.normalize();
double invDc = 1.0 / plan.getD();
//Create the homography
vpMatrix cdHc;
vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
cdHc /= cdHc[2][2];
//Create the 2D homography
vpMatrix cdGc = cam.get_K() * cdHc * cam.get_K_inverse();
//Points displacement
std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
for( ; iter != kltpoly->getCurrentPoints().end(); iter++){
vpColVector cdp(3);
cdp[0] = iter->second.get_j(); cdp[1] = iter->second.get_i(); cdp[2] = 1.0;
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Point2f p((float)cdp[0], (float)cdp[1]);
init_pts.push_back(p);
init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
#else
init_pts[iter_pts].x = (float)cdp[0];
init_pts[iter_pts].y = (float)cdp[1];
init_ids[iter_pts] = (kltpoly->getCurrentPointsInd())[(size_t)iter->first];
#endif
double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
cdp[0] = 0.0;
cdp[1] = 0.0;
throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
}
cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
//Set value to the KLT tracker
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
guess_pts.push_back(p_guess);
#else
guess_pts[iter_pts].x = (float)cdp[0];
guess_pts[iter_pts++].y = (float)cdp[1];
#endif
}
}
}
vpImageConvert::convert(I, cur);
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
tracker.setInitialGuess(init_pts, guess_pts, init_ids);
#else
tracker.setInitialGuess(&init_pts, &guess_pts, init_ids, iter_pts);
if(init_pts) cvFree(&init_pts);
init_pts = NULL;
if(guess_pts) cvFree(&guess_pts);
guess_pts = NULL;
if(init_ids)cvFree(&init_ids);
init_ids = NULL;
#endif
bool reInitialisation = false;
if(!useOgre)
faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
else{
#ifdef VISP_HAVE_OGRE
faces.setVisibleOgre(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
#else
faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
#endif
}
cam.computeFov(I.getWidth(), I.getHeight());
if(useScanLine){
faces.computeClippedPolygons(cdMo,cam);
faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
}
for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
kltpoly = *it;
if(kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2){
kltpoly->polygon->computePolygonClipped(cam);
kltpoly->init(tracker);
}
}
cMo = cdMo;
c0Mo = cMo;
ctTc0.eye();
}
}
// Coprocessor Data (CDP) Instructions, type = Align:cp_num = p0,opcode2 = 001
// Opcode1 = 0 v v v
void waligni(WMMXRegister wRd, WMMXRegister wRn,
WMMXRegister wRm, int Imm3, Condition cond = al) {
Imm3 = Imm3 & 0x7;
cdp(p0, Imm3, (CRegister)wRd, (CRegister)wRn, (CRegister)wRm, 1, cond);
}
// Opcode1 = 3
void wandn(WMMXRegister wRd, WMMXRegister wRn,
WMMXRegister wRm, Condition cond = al) {
cdp(p0, 3, (CRegister)wRd, (CRegister)wRn, (CRegister)wRm, 0, cond);
}
// Opcode1 = 1
void wxor(WMMXRegister wRd, WMMXRegister wRn,
WMMXRegister wRm, Condition cond = al) {
cdp(p0, 1, (CRegister)wRd, (CRegister)wRn, (CRegister)wRm, 0, cond);
}
void DAQRichBlock::DecodeRich(integer length,int16u *p,int side,int secondary){
//
// This functions perform the actual decoding with the information
// of the side and if it is a primary of secondary block
//
int16u *pointer=p;
int CDPFound; // Counter for the number of CDP found
int low_gain[RICH_PMTperCDP][RICnwindows]; // Buffer to store low gain information just in case it is needed
for(CDPFound=0;;CDPFound++){
#ifdef __AMSDEBUG__
if(AMSFFKEY.Debug>1)
cout<<"Pointer at "<<pointer-p<<" fence in "<<length-1<<" fence content "<<*(p-1+length)<<endl;
#endif
if(pointer>=p-1+length) break; // Last fragment processed
FragmentParser cdp(pointer);
if(cdp.status.errors) Do(kCDPError);
int CDP=cdp.status.slaveId;
if(secondary) { if(CDP==1) CDP=9; else if(CDP==9) CDP=1; }
// Process data
// if(!cdp.status.isRaw && !cdp.status.isCompressed) Do(kCalibration);
if(cdp.status.isRaw){
// Prepare some nice histograms
/*
if(daq_histograms==0){
daq_histograms=new TH1F*[RICmaxpmts*RICnwindows*2]; // pixels*gains*pmts
for(int pmt=0;pmt<RICmaxpmts;pmt++)
for(int pixel=0;pixel<RICnwindows;pixel++)
for(int gain=0;gain<2;gain++){
char histo_name[1024];
sprintf(histo_name,"Rich_PMT_%i_pixel_%i_gain_%i",pmt,pixel,gain);
daq_histograms[gain+2*pixel+2*RICnwindows*pmt]=new TH1F(histo_name,histo_name,4096,-0.5,4095.5);
}
}
*/
if(cdp.length!=1+ // Status word
RICH_PMTperCDP*RICnwindows*2) Do(kCDPRawTruncated);
// Fill raw information
// Simple loop getting all the information
DSPRawParser channel(cdp.data);
do{
//daq_histograms[channel.gain+2*channel.pixel+2*RICnwindows*channel.pmt]->Fill(channel.counts);
// First comes the low gain, the high gain then
if(channel.gain==0) {low_gain[channel.pmt][channel.pixel]=channel.counts;}
else{
// Get the geom ID for this channel
int physical_cdp=Links[side][CDP];
if(physical_cdp!=-1){
int geom_id=RichPMTsManager::GetGeomPMTIdFromCDP(physical_cdp,channel.pmt);
if(geom_id>=0){
// Get the pixel geom id, substract the pedestal, check that
// it is above it and use low gain if necessary
int pixel_id=RichPMTsManager::GetGeomChannelID(geom_id,channel.pixel);
if(RichPMTsManager::Status(geom_id,pixel_id)%10==Status_good_channel){ // Channel is OK
int mode=1; // High gain
int counts=channel.counts;
if(channel.counts>RichPMTsManager::GainThreshold(geom_id,pixel_id)){
counts=low_gain[channel.pmt][channel.pixel];
mode=0;
}
float threshold=RichPMTsManager::PedestalThreshold(geom_id,pixel_id,mode)*
RichPMTsManager::PedestalSigma(geom_id,pixel_id,mode);
// Add the hit
int channel_geom_number=RichPMTsManager::PackGeom(geom_id,pixel_id);
counts-=int(RichPMTsManager::Pedestal(geom_id,pixel_id,mode));
if(!cdp.status.isCompressed) // Just in case it is Mixed mode
if(counts>threshold || mode==0)
AMSEvent::gethead()->
addnext(
AMSID("AMSRichRawEvent",0),
new AMSRichRawEvent(channel_geom_number,
counts,
mode?0:gain_mode)
);
}
}
}
}
}while(channel.Next());
}
if(cdp.status.isCompressed){
// if(cdp.status.isRaw)Do(kMixedMode); // Nothing to do indeed
// Fill compressed mode if something to fill
if(cdp.length-1>0){
DSPCompressedParser channel(cdp.data,cdp.length-1);
do{
int physical_cdp=Links[side][CDP];
if(physical_cdp!=-1){
int geom_id=RichPMTsManager::GetGeomPMTIdFromCDP(physical_cdp,channel.pmt);
// if(geom_id<0) Do(kWrongCDPChannelNumber);
if(geom_id<0) continue;
// Get the pixel geom id, substract the pedestal, check that
// it is above it and use low gain if necessary
int pixel_id=RichPMTsManager::GetGeomChannelID(geom_id,channel.pixel);
if(RichPMTsManager::Status(geom_id,pixel_id)%10==Status_good_channel){ // Channel is OK
int mode=channel.gain; // High gain
int counts=channel.counts;
if(channel.inconsistent){
// Substract the Gx1 pedestal
counts-=int(RichPMTsManager::Pedestal(geom_id,pixel_id,0));
}
int channel_geom_number=RichPMTsManager::PackGeom(geom_id,pixel_id);
#ifdef __INSPECT__
cout<<"ADDING HIT IN COMPRESSED MODE: "<<endl
<<" PMT GEOM ID "<<channel_geom_number<<endl
<<" COUNTS "<<counts<<endl
<<" HIGH GAIN "<<mode<<endl;
#endif
AMSEvent::gethead()->addnext(AMSID("AMSRichRawEvent",0),
new AMSRichRawEvent(channel_geom_number,
counts,
mode?0:gain_mode));
}
}
}while(channel.Next());
}
}
pointer=cdp.next;
}
}