void SensorModel::scResample(sensor_msgs::LaserScan *scan, vector<Pose>& poses) { if(likelihoodField == NULL) return; for(int i = 0; i < poses.size(); i++) { // ROS_INFO("Computing weights for pose[%d]", i); if(!computeWeight(poses[i], scan)) return; } normalizeWeights(poses); }
void weightWindow::compute(const dimension_type i, const dimension_type j, const genericWindow<elevation_type>& elevwin, const direction_type dir, const int trustdir) { elevation_type elev_crt, elev_neighb, e_diff; dimension_type i_neighb, j_neighb; /* initialize all weights to 0 */ init(); elev_crt = elevwin.get(); assert(!is_nodata(elev_crt)); /* map direction to neighbors */ directionWindow dirwin(dir); /* compute weights of the 8 neighbours */ int skipit = 0; for (short di = -1; di <= 1; di++) { for (short dj = -1; dj <= 1; dj++) { /* grid coordinates and elevation of neighbour */ i_neighb = i + di; j_neighb = j + dj; elev_neighb = elevwin.get(di, dj); e_diff = (elevation_type)(elev_crt - elev_neighb); skipit = ((di ==0) && (dj==0)); skipit |= (elev_crt < elev_neighb); /* skipit |= (elev_neighb == edge_nodata); ?? */ if (!trustdir) { dirwin.correctDirection(di,dj,skipit, i,j, elev_crt,dir,elev_neighb); } /* if direction points to it then compute its weight */ if (dirwin.get(di,dj) == true) { computeWeight(di,dj, elev_crt, elev_neighb); } } /* for dj */ } /* for di */ normalize(); /* normalize the weights */ #ifdef CHECK_WEIGHTS cout <<"weights: ["; for (int l=0;l<9;l++) cout << form("%3.2f ",weight.get(l)); cout <<"]\n"; #endif };
//Compute the Weignted laplacian void LaplacianMesh :: weightedLaplacian() { //Compute weight MatrixXd A = computeWeight(); int N = P2P_Neigh.size(); MatrixXd D; D=MatrixXd::Zero(N,N); //Iterates for all P2P_neigh set<int>::iterator it; for(int i=0;i<P2P_Neigh.size(); i++) { //Sum all the weights to compute D matrix D(i,i)=A.row(i).sum(); } laplacianMatrix = D-A; }
void Correlation::computeEffectiveSize() { if(nbsample==0) { cerr << "ERROR: in Correlation::computeEffectiveSize, chain size is 0, exit\n"; cerr.flush(); exit(0); } else { computeWeight(); double sum; for(int i=0;i<nbparameter;i++) { sum=1;//covparam[i][0];*covparam[i][0]; for(int j=1;j<nbsample/2;j++) { sum+=2*weight[j]*covnorm[i][j]; } effectiveSize[i]=nbsample/sum; if(sum<1) effectiveSize[i]=nbsample; } } }
template <class T> ossimRefPtr<ossimImageData> ossimFeatherMosaic::combine( T, const ossimIrect& tileRect, ossim_uint32 resLevel) { ossimRefPtr<ossimImageData> currentImageData; ossim_uint32 band; long upperBound = theTile->getWidth()*theTile->getHeight(); long offset = 0; long row = 0; long col = 0; long numberOfTilesProcessed = 0; float *sumBand = static_cast<float*>(theAlphaSum->getBuf()); float *bandRes = NULL; ossimIpt point; theAlphaSum->fill(0.0); theResult->fill(0.0); T** srcBands = new T*[theLargestNumberOfInputBands]; T** destBands = new T*[theLargestNumberOfInputBands]; ossim_uint32 layerIdx = 0; currentImageData = getNextTile(layerIdx, 0, tileRect, resLevel); if(!currentImageData.valid()) { delete [] srcBands; delete [] destBands; return currentImageData; } ossim_uint32 minNumberOfBands = currentImageData->getNumberOfBands(); for(band = 0; band < minNumberOfBands; ++band) { srcBands[band] = static_cast<T*>(currentImageData->getBuf(band)); destBands[band] = static_cast<T*>(theTile->getBuf(band)); } // if the src is smaller than the destination in number // of bands we will just duplicate the last band. for(;band < theLargestNumberOfInputBands; ++band) { srcBands[band] = srcBands[minNumberOfBands - 1]; destBands[band] = static_cast<T*>(theTile->getBuf(band)); } // most of the time we will not overlap so let's // copy the first tile into destination and check later. // for(band = 0; band < theTile->getNumberOfBands();++band) { T* destBand = destBands[band]; T* srcBand = srcBands[band]; if(destBand&&srcBand) { for(offset = 0; offset < upperBound;++offset) { *destBand = *srcBand; ++srcBand; ++destBand; } } } theTile->setDataObjectStatus(currentImageData->getDataObjectStatus()); while(currentImageData.valid()) { ossimDataObjectStatus currentStatus = currentImageData->getDataObjectStatus(); point = currentImageData->getOrigin(); long h = (long)currentImageData->getHeight(); long w = (long)currentImageData->getWidth(); if( (currentStatus != OSSIM_EMPTY) && (currentStatus != OSSIM_NULL)) { ++numberOfTilesProcessed; offset = 0; minNumberOfBands = currentImageData->getNumberOfBands(); for(band = 0; band < minNumberOfBands; ++band) { srcBands[band] = static_cast<T*>(currentImageData->getBuf(band)); } // if the src is smaller than the destination in number // of bands we will just duplicate the last band. for(;band < theLargestNumberOfInputBands; ++band) { srcBands[band] = srcBands[minNumberOfBands - 1]; } if(currentStatus == OSSIM_PARTIAL) { for(row = 0; row < h; ++row) { for(col = 0; col < w; ++col) { if(!currentImageData->isNull(offset)) { double weight = computeWeight(layerIdx, ossimDpt(point.x+col, point.y+row)); for(band = 0; band < theLargestNumberOfInputBands; ++band) { bandRes = static_cast<float*>(theResult->getBuf(band)); bandRes[offset] += (srcBands[band][offset]*weight); } sumBand[offset] += weight; } ++offset; } } } else { offset = 0; for(row = 0; row < h; ++row) { for(col = 0; col < w; ++col) { double weight = computeWeight(layerIdx, ossimDpt(point.x+col, point.y+row)); for(band = 0; band < theLargestNumberOfInputBands; ++band) { bandRes = static_cast<float*>(theResult->getBuf(band)); bandRes[offset] += (srcBands[band][offset]*weight); } sumBand[offset] += weight; ++offset; } } } } currentImageData = getNextTile(layerIdx, tileRect, resLevel); } upperBound = theTile->getWidth()*theTile->getHeight(); if(numberOfTilesProcessed > 1) { const double* minPix = theTile->getMinPix(); const double* maxPix = theTile->getMaxPix(); const double* nullPix= theTile->getNullPix(); for(offset = 0; offset < upperBound;++offset) { for(band = 0; band < theTile->getNumberOfBands();++band) { T* destBand = static_cast<T*>(theTile->getBuf(band)); float* weightedBand = static_cast<float*>(theResult->getBuf(band)); // this should be ok to test 0.0 instead of // FLT_EPSILON range for 0 since we set it. if(sumBand[offset] != 0.0) { weightedBand[offset] = (weightedBand[offset])/sumBand[offset]; if(weightedBand[offset]<minPix[band]) { weightedBand[offset] = minPix[band]; } else if(weightedBand[offset] > maxPix[band]) { weightedBand[offset] = maxPix[band]; } } else { weightedBand[offset] = nullPix[band]; } destBand[offset] = static_cast<T>(weightedBand[offset]); } } theTile->validate(); } delete [] srcBands; delete [] destBands; return theTile; }
/* int */ void findAllCircle(Edge **C, int **F, int **remain, int *stack, int *sTop, int nNodes, int sink) { int u = sink; /* 表示当前的顶点 */ /* bool ret = false; */ int *circle, cLen; int *preflow; // 显示每次消去一环所消耗的时间 // struct timeval NC_begin; // struct timeval NC_end; // int NC_count = 0; // double time; circle = (int *)calloc(nNodes+1, sizeof(int)); /* cLen = 0; */ preflow = (int *)malloc(sizeof(int)); /* *preflow = INFINITE; */ labRedo: // 显示消去一环的时间消耗 // gettimeofday(&NC_begin, NULL); for (;;) { while (u != -1) { if (contain(stack, *sTop, u) == -1) { stack[(*sTop)++] = u; u = getFirstNeighbor(remain, nNodes, u); } else { int v = contain(stack, *sTop, u); stack[(*sTop)++] = u; cLen = 0; while (v < *sTop) circle[cLen++] = stack[v++]; /* int i; */ /* for (i = 0; i < cLen; i++) */ /* printf("%d\t", circle[i]); */ /* printf("\n"); */ *preflow = INFINITE; /* 此处将cLen > 4改为cLen > 3 */ if (cLen > 3 && computeWeight(C, F, remain, circle, cLen, preflow) < 0) { /* updateFlow(F, circle, cLen, preflow); */ updateFlowRemain(F, remain, circle, cLen, preflow); *sTop = 0; u = circle[0]; cLen = 0; // 显示消去一环的时间消耗 // NC_count++; // gettimeofday(&NC_end, NULL); // time = (NC_end.tv_sec-NC_begin.tv_sec)*1000000 + NC_end.tv_usec-NC_begin.tv_usec; // printf("%d\t%15.2f\n", NC_count, time); goto labRedo; /* ret = true; */ } break; } } if (*sTop != 0) { int v = stack[--(*sTop)]; if (*sTop != 0) { u = stack[(*sTop)-1]; u = getNextNeighbor(remain, nNodes, u, v); } else goto labelReturn; /* return ret; */ } else goto labelReturn; /* return ret; */ } labelReturn: // 显示消去一环的时间消耗 // gettimeofday(&NC_end, NULL); // time = (NC_end.tv_sec-NC_begin.tv_sec)*1000000 + NC_end.tv_usec-NC_begin.tv_usec; // printf("end\t%15.2f\n", time); free(circle); free(preflow); /* return ret; */ }
//------------------------------------------------------------------------------ //! const Vector<Puppeteer::PositionalConstraint>& ConstraintConverter::computePoseConstraints( const uint poseID ) { _constraints.clear(); for( uint i = 0; i < _contactLists.size(); ++i ) { const ContactList& contactList = _contactLists[i]; if( contactList.empty() ) continue; const Contact* first = NULL; const Contact* next = NULL; // 1. Find first and last contacts. ContactList::ContactContainer::ConstIterator curC = contactList.contacts().begin(); ContactList::ContactContainer::ConstIterator endC = contactList.contacts().end(); for( ; curC != endC; ++curC ) { if( (*curC).start() <= poseID ) { // The first is the latest one which happens before the specified poseID. // We keep assigning until we go beyond that point. first = &(*curC); } else { // The next is the first one which happens after the specified poseID. // We stop as soon as we find such a case. next = &(*curC); break; } } #define ADD_CONSTRAINT(p, w) \ if( w > 0.0f ) \ { \ _constraints.pushBack( \ Puppeteer::PositionalConstraint( \ p, w, contactList.boneID() \ ) \ ); \ } // 2. Handle the various cases. if( first != NULL ) { if( poseID <= first->end() ) { // The contact is currently active, so use weight of 1. ADD_CONSTRAINT( first->target(), 1.0f ); continue; } Vec3f bonePos = getBonePosition( contactList.boneID() ); float firstW = computeWeight( bonePos, first->endPosition(), contactList.influenceRadius() ); if( next != NULL ) { // Sitting between 2 candidates (pick the best one). float nextW = computeWeight( bonePos, next->startPosition(), contactList.influenceRadius() ); if( firstW > nextW ) { ADD_CONSTRAINT( first->target(), firstW ); } else { ADD_CONSTRAINT( next->target(), nextW ); } } else { // End of the animation (only a first). ADD_CONSTRAINT( first->target(), firstW ); } } else { // Beginning of the animation (only a last). // (guaranteed since we checked for emptiness at the beginning). Vec3f bonePos = getBonePosition( contactList.boneID() ); float nextW = computeWeight( bonePos, next->startPosition(), contactList.influenceRadius() ); ADD_CONSTRAINT( next->target(), nextW ); } // else, none are valid... } #undef ADD_CONSTRAINT return _constraints; }