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;
}
