void WaveEquationSolver::PerturbationIdx(int i, int j)
{
if( m_xCount < 10 || m_yCount < 10 )
return;
if( i<3 ) i = 3;
if( j<3 ) j = 3;
if( i > m_xCount-4 ) i = m_xCount-4;
if( j > m_yCount-4 ) j = m_yCount-4;
float dist;
float height;
//
// Возмущение по косинусу
//
for( int ri = -2; ri <= 2; ++ri )
for( int rj = -2; rj <= 2; ++rj )
{
float& u1 = m_u1[ Idx(i+ri,j+rj) ];
float& u2 = m_u1[ Idx(i+ri,j+rj) ];
dist = sqrt( (float)ri*ri + (float)rj*rj );
height = cos( dist/3.0f * (float)M_PI ) + 1;
height *= m_perturbMultipler;
u1 += height;
u2 = u1;
}
}
inline ssvu::EnableIf < I<sizeof...(TArgs)> fromTpl(Obj& mObj,
const ssvu::Tpl<TArgs...>& mTpl)
{
Converter<TplArg<I, decltype(mTpl)>>::toObj(
mObj[Idx(I)], std::get<I>(mTpl));
fromTpl<I + 1, TArgs...>(mObj, mTpl);
}
//
// Zwraca czesc zalezna od kata \theta w sferycznym ukladzie wspolrzednych
//
double ShReal::Plm(int l, int m, double theta) const
{
const int idx = Idx(l, m);
double v;
assert(l >= 0 && l <= 3);
assert(labs(m) <= l);
switch(idx)
{
// l = 0
case 0: v = 1; break;
// l = 1
case 1: v = sin(theta); break; // m = -1
case 2: v = cos(theta); break; // m = 0
case 3: v = sin(theta); break; // m = 1
// l = 2
case 4: v = sin(theta); v = v * v; break; // m = -2
case 5: v = sin(2 * theta); break; // m = -1
case 6: v = 1 + 3 * cos(2 * theta); break; // m = 0
case 7: v = sin(2 * theta); break; // m = 1
case 8: v = sin(theta); v = v * v; break; // m = 2
// l = 3
case 15: // m = +3
case 9: // m = -3
v = sin(theta);
v = v * v * v;
break;
case 14: // m = +2
case 10: // m = -2
v = sin(theta);
v = cos(theta) * v * v;
break;
case 13: // m = +1
case 11: // m = -1
v = sin(theta) + 5 * sin(3 * theta);
break;
case 12: // m = 0
v = 3 * cos(theta) + 5 * cos(3 * theta);
break;
default: assert(0); v = 0; break;
}
return m_slm[idx] * v;
}
void WaveEquationSolver::CalculateNormal(int i, int j, float* n)
{
if( i>0 && j>0 && i<m_xCount-1 && j<m_yCount-1 )
{
Vec3f X1 = MakeVec<float>( (i-1)*m_gridStep, j*m_gridStep, m_u1[ Idx(i-1,j) ] );
Vec3f X2 = MakeVec<float>( (i+1)*m_gridStep, j*m_gridStep, m_u1[ Idx(i+1,j) ] );
Vec3f Y1 = MakeVec<float>( i*m_gridStep, (j-1)*m_gridStep, m_u1[ Idx(i,j-1) ] );
Vec3f Y2 = MakeVec<float>( i*m_gridStep, (j+1)*m_gridStep, m_u1[ Idx(i,j+1) ] );
Vec3f N = CrossProduct(X2 - X1, Y2 - Y1);
n[0] = N[0];
n[1] = N[1];
n[2] = N[2];
return;
}
n[0] = 0;
n[1] = 0;
n[2] = 1;
}
int main(int argc, char **argv)
{
// (when V-REP launches this executable, V-REP will also provide the argument list)
//numero de argumentos que mande (se excluye el fantasma que el manda solo)
if (argc>=1)
{
//str=atoi(argv[1]); //N obstaculos
}
else
{
printf("Indique argumentos!\n");
sleep(5000);
return 0;
}
infoMapa1.coordenadasCelda_y = atoi(argv[1])-1; // Reajuste de indice
infoMapa1.coordenadasCelda_x = atoi(argv[2])-1; // Reajuste de indice
infoMapa2.coordenadasCelda_y = atoi(argv[3])-1; // Reajuste de indice
infoMapa2.coordenadasCelda_x = atoi(argv[4])-1; // Reajuste de indice
infoMapa1.valorCelda = atoi(argv[5]);
infoMapa2.finalCeldas = atoi(argv[6]);
usleep(1000);
// Create a ROS node. The name has a random component:
int _argc = 0;
char** _argv = NULL;
std::string nodeName("CeldaDeMapa");
std::string Idy(boost::lexical_cast<std::string>(atoi(argv[1])));
std::string Idx(boost::lexical_cast<std::string>(atoi(argv[2])));
Idy+=Idx;
nodeName+=Idy;
ros::init(_argc,_argv,nodeName.c_str());
//ROS_INFO("Hola Celdas!");
ros::NodeHandle n;
ros::Publisher chatter_pub1 = n.advertise<camina::InfoMapa>("InfoDeMapa1", 1000);
ros::Publisher chatter_pub2 = n.advertise<camina::InfoMapa>("InfoDeMapa2", 1000);
ros::Subscriber subInfo=n.subscribe("/vrep/info",1000,infoCallback);
//ros::Rate wait_rate(atoi(argv[1])+atoi(argv[2]));
ros::Rate wait_rate(2);
wait_rate.sleep(); //MUY NECESARIO... para dar tiempo a mensajes
chatter_pub1.publish(infoMapa1);
wait_rate.sleep(); //MUY NECESARIO... para dar tiempo a mensajes
chatter_pub2.publish(infoMapa2);
//ROS_INFO("Adios1!");
ros::shutdown();
return 0;
}
void WaveEquationSolver::SolveOneStep()
{
float dudx;
float dudy;
float a2 = m_speed * m_speed;
float t2 = m_timeStep * m_timeStep;
float h2 = m_gridStep * m_gridStep;
//
// Проход по всем узлам, исключая граничные
//
for( int i = 1; i < m_xCount-1; ++i )
for( int j = 1; j < m_yCount-1; ++j )
{
dudx = m_u1[ Idx(i+1,j) ] - 2*m_u1[ Idx(i,j) ] + m_u1[ Idx(i-1,j) ];
dudy = m_u1[ Idx(i,j+1) ] - 2*m_u1[ Idx(i,j) ] + m_u1[ Idx(i,j-1) ];
m_u2[ Idx(i,j) ] = a2/h2 * (dudx + dudy) + (2*m_u1[ Idx(i,j) ] - m_u0[ Idx(i,j) ])/t2;
m_u2[ Idx(i,j) ] *= t2 * m_damping;
}
std::swap( m_u0, m_u1 );
std::swap( m_u1, m_u2 );
}
//
// Zwraca wartosc rzeczywistej harmoniki sferycznej w punkcie (x, y, z) w karezjanskim ukladzie wspolrzednych.
// Musi zachodzic zaleznosc r = sqrt(x*x + y*y + z*z)
// Promien "r" jest przekazywany aby uniknac wielokrotnego obliczania pierwiastka
//
double ShReal::Get(int l, int m, double x, double y, double z, double r) const
{
const int idx = Idx(l, m);
//const double r2 = r * r, r3 = r * r * r;
double v;
assert(l >= 0 && l <= 3);
assert(labs(m) <= l);
if(r < FLT_EPSILON)
return m_clm[0];
switch(idx)
{
// l = 0
case 0: v = 1; break;
// l = 1
case 1: v = y / r; break; // m = -1
case 2: v = z / r; break; // m = 0
case 3: v = x / r; break; // m = 1
// l = 2
case 4: v = x * y / (r*r); break; // m = -2
case 5: v = y * z / (r*r); break; // m = -1
case 6: v = (3 * z * z - r*r) / (r*r); break; // m = 0
case 7: v = x * z / (r*r); break; // m = 1
case 8: v = (x - y) * (x + y) / (r*r); break; // m = 2
// l = 3
case 9: v = y * (3 * x * x - y * y) / (r*r*r); break; // m = -3
case 10: v = x * y * z / (r*r*r); break; // m = -2
case 11: v = y * (5 * z * z - r * r) / (r*r*r); break; // m = -1
case 12: v = z * (5 * z * z - 3 * r * r) / (r*r*r); break; // m = 0
case 13: v = x * (5 * z * z - r * r) / (r*r*r); break; // m = 1
case 14: v = z * (x - y) * (x + y) / (r*r*r); break; // m = 2
case 15: v = x * (x * x - 3 * y * y) / (r*r*r); break; // m = 3
default: assert(0); v = 0; break;
}
return m_clm[idx] * v;
}
void KernFeature::makeCoverage()
{
if ( GPOSTableRaw.isEmpty() )
return;
quint16 FeatureList_Offset= toUint16 ( 6 );
quint16 LookupList_Offset = toUint16 ( 8 );
// Find the offsets of the kern feature tables
quint16 FeatureCount = toUint16 ( FeatureList_Offset );;
QList<quint16> FeatureKern_Offset;
for ( quint16 FeatureRecord ( 0 ); FeatureRecord < FeatureCount; ++ FeatureRecord )
{
int rawIdx ( FeatureList_Offset + 2 + ( 6 * FeatureRecord ) );
quint32 tag ( FT_MAKE_TAG ( GPOSTableRaw.at ( rawIdx ),
GPOSTableRaw.at ( rawIdx + 1 ),
GPOSTableRaw.at ( rawIdx + 2 ),
GPOSTableRaw.at ( rawIdx + 3 ) ) );
if ( tag == TTAG_kern )
{
FeatureKern_Offset << ( toUint16 ( rawIdx + 4 ) + FeatureList_Offset );
}
}
// Extract indices of lookups for feture kern
QList<quint16> LookupListIndex;
foreach ( quint16 kern, FeatureKern_Offset )
{
quint16 LookupCount ( toUint16 ( kern + 2 ) );
for ( int llio ( 0 ) ; llio < LookupCount; ++llio )
{
quint16 Idx ( toUint16 ( kern + 4 + ( llio * 2 ) ) );
if ( !LookupListIndex.contains ( Idx ) )
{
LookupListIndex <<Idx ;
}
}
}
int pcDenoise::pcDecompose()
{
if( cloud->empty())
{
std::cout<< "The input point cloud seems to be empty, try invoke setCloud or read it from pcd file.";
return -1;
}
pcType temp_cloud(new pcl::PointCloud<PT>);
int K = 100;
pcl::KdTreeFLANN<PT> kdtree;
kdtree.setInputCloud(cloud);
std::vector<int> Idx(K);
std::vector<float> Dist(K);
double mx=0;
double my=0;
double mz=0;
for( int i=0; i<cloud->width;i++)
{
mx += (cloud->points.at(i).x);
my += (cloud->points.at(i).y);
mz += (cloud->points.at(i).z);
}
mx/=cloud->width;
my/=cloud->width;
mz/=cloud->width;
for( int i=0; i<cloud->width;i++)
{
kdtree.nearestKSearch (i, K, Idx, Dist);
int neighbourIdx = 0;
double xx=0;
double yy=0;
double zz=0;
for( int j=0; j<K; j++)
{
neighbourIdx = Idx[j];
xx += (cloud->points.at(neighbourIdx).x);
yy += (cloud->points.at(neighbourIdx).y);
zz += (cloud->points.at(neighbourIdx).z);
}
xx/=K;
yy/=K;
zz/=K;
//xx-=cloud->points.at(i).x;
//yy-=cloud->points.at(i).y;
//zz-=cloud->points.at(i).z;
xx-=mx;
yy-=my;
zz-=mz;
PT* temp = new PT();
temp->x = xx;
temp->y = yy;
temp->z = zz;
temp->r = (cloud->points.at(i).r);
temp->g = (cloud->points.at(i).g);
temp->b = (cloud->points.at(i).b);
temp_cloud->push_back( *temp );
}
cloud->clear();
pcl::copyPointCloud(*temp_cloud,*cloud);
return 0;
}
double Processor::ULatProcToIface (UShort pIdx, bool dynamic, UShort pSize, UShort subnIdx)
{
return (pIdx == Idx()) ? 0.0 : MAX_DOUBLE;
}
int Processor::DistanceProcToIface (UShort pIdx)
{
return (pIdx == Idx()) ? 0 : MAX_INT;
}
bool Processor::HasProcessor (UShort idx)
{
return (idx == Idx()) ? true : false;
}
//
// Zwraca wartosc wspolczynnika unormowania
//
double ShReal::Clm(int l, int m) const
{
return m_clm[Idx(l, m)];
}
cv::Mat PM_type::nms(const cv::Mat &boxes, float overlap)
//% Non-maximum suppression.
//% pick = nms(boxes, overlap)
//%
//% Greedily select high-scoring detections and skip detections that are
//% significantly covered by a previously selected detection.
//%
//% Return value
//% pick Indices of locally maximal detections
//%
//% Arguments
//% boxes Detection bounding boxes (see pascal_test.m)
//% overlap Overlap threshold for suppression
//% For a selected box Bi, all boxes Bj that are covered by
//% more than overlap are suppressed. Note that 'covered' is
//% is |Bi \cap Bj| / |Bj|, not the PASCAL intersection over
//% union measure.
{
if( boxes.empty() )
return boxes;
cv::Mat x1 = boxes.col(0);
cv::Mat y1 = boxes.col(1);
cv::Mat x2 = boxes.col(2);
cv::Mat y2 = boxes.col(3);
cv::Mat s = boxes.col(boxes.cols - 1);
cv::Mat area = x2 - x1 + 1;
area = area.mul(y2-y1+1);
vector<int> Ind( s.rows, 0 );
cv::Mat Idx(s.rows, 1, CV_32SC1, &Ind[0]);
sortIdx( s, Idx, CV_SORT_EVERY_COLUMN+CV_SORT_ASCENDING );
vector<int> pick;
while( !Ind.empty() ){
int last = Ind.size() - 1;
int i = Ind[last];
pick.push_back(i);
vector<int> suppress( 1, last );
for( int pos=0; pos<last; pos++ ){
int j = Ind[pos];
float xx1 = std::max(x1.at<float>(i), x1.at<float>(j));
float yy1 = std::max(y1.at<float>(i), y1.at<float>(j));
float xx2 = std::min(x2.at<float>(i), x2.at<float>(j));
float yy2 = std::min(y2.at<float>(i), y2.at<float>(j));
float w = xx2-xx1+1;
float h = yy2-yy1+1;
if( w>0 && h>0 ){
// compute overlap
float area_intersection = w * h;
float o1 = area_intersection / area.at<float>(j);
float o2 = area_intersection / area.at<float>(i);
float o = std::max(o1,o2);
if( o>overlap )
suppress.push_back(pos);
}
}
std::set<int> supp( suppress.begin(), suppress.end() );
vector<int> Ind2;
for( int i=0; i!=Ind.size(); i++ ){
if( supp.find(i)==supp.end() )
Ind2.push_back( Ind[i] );
}
Ind = Ind2;
}
cv::Mat ret(pick.size(), boxes.cols, boxes.type());
for( unsigned i=0; i<pick.size(); i++ )
boxes.row( pick[i] ).copyTo( ret.row(i) );
return ret;
}
HDINLINE
const typename mpl::at<Childs, Idx>::type&
child(Idx) const
{
return Base::at(Idx());
}
