void plat_x2c(void *info, __u64 ts, __u64 latency)
{
double now = TO_SEC(ts);
double lat = TO_SEC(latency);
struct plat_info *pp = info;
if (pp == NULL) return;
if (pp->first_ts == -1.0) {
pp->first_ts = pp->last_ts = now;
pp->nl = 1;
pp->tl = lat;
} else if ((now - pp->first_ts) >= plat_freq) {
double delta = pp->last_ts - pp->first_ts;
fprintf(pp->fp, "%lf %lf\n",
pp->first_ts + (delta / 2), pp->tl / pp->nl);
pp->first_ts = pp->last_ts = now;
pp->nl = 1;
pp->tl = lat;
} else {
pp->last_ts = now;
pp->nl += 1;
pp->tl += lat;
}
}
const int salad_predict_callback(data_t* data, const size_t n, void* const usr)
{
assert(data != NULL);
assert(n > 0);
assert(usr != NULL);
predict_t* const x = (predict_t*) usr;
struct timeval start, end;
gettimeofday(&start, NULL);
for (size_t i = 0; i < n; i++)
{
x->scores[i] = x->fct(&x->param, data[i].buf, data[i].len);
}
// Clock the calculation procedure
gettimeofday(&end, NULL);
double diff = TO_SEC(end) -TO_SEC(start);
x->totalTime += diff;
// Write scores
char buf[0x100];
#ifdef GROUPED_INPUT
if (x->config->group_input)
{
group_t* prev = data[0].meta;
fputs(TO_STRING(x->scores[0]), x->fOut);
for (size_t j = 1; j < n; j++)
{
fputs(prev == data[j].meta ? " " : "\n", x->fOut);
fputs(TO_STRING(x->scores[j]), x->fOut);
prev = data[j].meta;
}
}
else
#endif
{
for (size_t j = 0; j < n; j++)
{
fputs(TO_STRING(x->scores[j]), x->fOut);
fputs("\n", x->fOut);
}
}
return EXIT_SUCCESS;
}
//--------------------------------------------------------------------------------------------------------------------------------------
Emitter::Emitter(const vector3f& pos,
const vector3f& posNoise,
const vector3f& dir,
const vector3f& acce,
const vector2f& speedPmill,
float endspeed,
const vector3f& angle,
const Color& begin1,
const Color& begin2,
const Color& end1,
const Color& end2,
const vector2f& clrpow,
const vector2d& life,
const vector2f& alphaPow,
const vector2f& sizebegin,
const vector2f& sizeend,
const vector2f& sizepow,
const vector2d& countSec,
const int emitterlife,
std::string texName)
:ISceneNode( ISceneNode::RS_TRANSPARENT ),
LocatableObject( this ),
m_PositionNoise(posNoise),
m_Speed(speedPmill),
m_Direction(dir),
m_Acceleration(acce),
m_Angle(angle),
m_BeginColorA(begin1),
m_BeginColorB(begin2),
m_EndColorA(end1),
m_EndColorB(end2),
m_LifeSpan(life),
m_BeginSize(sizebegin),
m_EndSize(sizeend),
m_CountSec(countSec),
m_uTimeSpan(0),
m_uLifedTime(0),
m_ActiveTime(0),
m_EmitterLife(emitterlife),
m_isEmit(true),
m_isForceStop(false),
m_isInview(false),
m_ColorPow(clrpow),
m_AlphaPow(alphaPow),
m_SizePow(sizepow),
m_EndSpeed(endspeed),
m_fResistance(((endspeed / speedPmill.m_y) - 1.0f )/static_cast<float>(life.m_y))
{
SetLocalPosition( pos );
m_Direction.NormalizeSelf();
float maxlife = Math::GetMax(life.m_x, life.m_y);
float maxcount = Math::GetMax(countSec.m_x, countSec.m_y);
//int maxParticle = static_cast<int>( ( ceil(TO_SEC( maxlife )) ) * maxcount );
int maxParticle = 0;
if ( -1 != emitterlife )
{
maxParticle = static_cast<int>( ( TO_SEC( emitterlife ) ) * ( TO_SEC( maxlife ) ) * maxcount );
}
else
{
maxParticle = static_cast<int>( ( TO_SEC( maxlife ) ) * maxcount );
}
m_ArraySize = maxParticle;
m_Index.SetMax(maxParticle);
m_pParticle = NEW Particle_ColorSizeForce[maxParticle+1];//此处分配的时候一定要多分配一个，防止glEnableVertexAttribArray越界访问，造成堆栈损坏
m_pRendBuffer = NEW RendBuffer( Device::RM_POINTS );
m_pVertexBuffer = NEW VertexBuffer( Device::MU_DYNAMIC );
m_pRendBuffer->SetVertexBuffer( m_pVertexBuffer );
m_pVertexBuffer->FlushVertexBuffer( m_ArraySize, &m_pParticle[0] );
//ushort* indies = NEW ushort[maxParticle];
//for ( int i = 0;i < maxParticle ; i ++ )
//{
// indies[i] = i;
//}
//m_pRendBuffer->MakeIndiesBuffer(indies, maxParticle, Device::MU_STATIC);
//SAFE_DELETE_ARRAY(indies);
//m_Tex = NEW Texture2D();
//m_Tex->LoadTexture( Device::PF_A8, texName);
m_isBeginChangeColor = m_BeginColorA == m_BeginColorB ? false : true;
m_isEndChangeColor = m_EndColorA == m_EndColorB ? false : true;
//材质
m_pMaterial = NEW Material;
m_pMaterial->SetNode( this );
m_pMaterial->SetShader( Pipeline::PT_LIGHTING, ShaderManage::ParticleShader );
m_pMaterial->LoadTexture( Material::ATT_TEX_DIFFUSE, Device::PF_R8G8B8A8, texName );
m_pMaterial->GetDrawState( Pipeline::PT_LIGHTING ).m_isDepthMask = false;
m_pMaterial->GetAlpahState( Pipeline::PT_LIGHTING ).m_isAlphaEnable = true;
//计算绑定盒子
//由于喷射角度关系，绑定盒子很难精确计算，将采用随机测试的方法在编辑器中输出
}
//--------------------------------------------------------------------------------------------------------------------------------------
void Emitter::Update(uint millisecond)
{
LocatableObject::Update( millisecond );
m_isInview = false;
if( !m_isForceStop )
{
if( isEmit() || m_uLifedTime <= m_EmitterLife + m_LifeSpan.m_y )//如果粒子还活着
{
aabbox3df worldbox = m_BindBox; //初始的绑定和不能变
worldbox.SetCenter( GetWorldPosition() );
if ( Engine::Instance().GetCamera()->GetFrustum().Intersect( worldbox ) )//判断是否可视
{
m_isInview = true;
}
//更新粒子
m_uLifedTime += millisecond;
if( m_isInview )
{
m_ActiveTime += millisecond;
if( m_isEmit )
{
m_isEmit = m_uLifedTime < m_EmitterLife || m_EmitterLife < 0;//计算是否继续发射
vector2d view = Engine::Instance().GetDevice()->GetResolutionSize();
float particlesize = ( view.m_x + view.m_y ) * 0.5;
float worldscale = Math::GetMax( GetWorldScale().m_x, Math::GetMax( GetWorldScale().m_y, GetWorldScale().m_z ) );
m_uTimeSpan += millisecond;
int randomNum = Math::Random(m_CountSec.m_x, m_CountSec.m_y);
float fnum = randomNum * TO_SEC( m_uTimeSpan );
int relaseNum = fnum;
if( 0 != relaseNum)
{
m_uTimeSpan = ( fnum - relaseNum ) / randomNum * m_uTimeSpan; //计算一下小数位置的粒子个数是对应多长时间差内的粒子个数，这个是降低误差用的
relaseNum = Math::GetMin( relaseNum , m_Index.Max() );
int beginIndex = m_Index;
for (int i = 0 ; i < relaseNum ; i++)
{
Quaternionf rotation;
rotation.YawPitchRoll(Math::fRandom(-m_Angle.m_y,m_Angle.m_y), Math::fRandom(-m_Angle.m_x,m_Angle.m_x) , Math::fRandom(-m_Angle.m_z,m_Angle.m_z));
//Matrix33f rotation;
//rotation.YawPitchRoll(fRandom(-m_Angle.m_y,m_Angle.m_y), fRandom(-m_Angle.m_x,m_Angle.m_x) , fRandom(-m_Angle.m_z,m_Angle.m_z));
vector3f Velocity = m_Direction * rotation * GetWorldRotation();
Velocity.NormalizeSelf();
Velocity *= ( Math::fRandom( m_Speed.m_x , m_Speed.m_y ) );
Color Beginclr, Endclr, clrInc;
if( m_isBeginChangeColor )
{
Beginclr = Color(Math::fRandom(m_BeginColorA.m_r, m_BeginColorB.m_r),Math::fRandom(m_BeginColorA.m_g, m_BeginColorB.m_g),Math::fRandom(m_BeginColorA.m_b, m_BeginColorB.m_b),Math::fRandom(m_BeginColorA.m_a, m_BeginColorB.m_a));
}
else
{
Beginclr = m_BeginColorA;
}
if( m_isEndChangeColor )
{
Endclr = Color(Math::fRandom(m_EndColorA.m_r, m_EndColorB.m_r),Math::fRandom(m_EndColorA.m_g, m_EndColorB.m_g),Math::fRandom(m_EndColorA.m_b, m_EndColorB.m_b),Math::fRandom(m_EndColorA.m_a, m_EndColorB.m_a));
}
else
{
Endclr = m_EndColorA;
}
if( m_PositionNoise == vector3f::Zero() )
{
m_pParticle[m_Index].Create(
GetWorldPosition(),
Beginclr,
Endclr,
Velocity * worldscale,
m_ActiveTime,
Math::Random(m_LifeSpan.m_x, m_LifeSpan.m_y),
Math::fRandom(m_BeginSize.m_x, m_BeginSize.m_y) * particlesize * worldscale,
Math::fRandom(m_EndSize.m_x, m_EndSize.m_y) * particlesize * worldscale,
Math::fRandom(m_ColorPow.m_x, m_ColorPow.m_y),
Math::fRandom(m_AlphaPow.m_x, m_AlphaPow.m_y),
Math::fRandom(m_SizePow.m_x, m_SizePow.m_y)
);
m_Index++;
}
else
{
m_pParticle[m_Index].Create(
GetWorldPosition() + vector3f( Math::fRandom( -m_PositionNoise.m_x, m_PositionNoise.m_x ), Math::fRandom( -m_PositionNoise.m_y, m_PositionNoise.m_y ), Math::fRandom( -m_PositionNoise.m_z, m_PositionNoise.m_z ) ) * worldscale,
Beginclr,
Endclr,
Velocity * worldscale,
m_ActiveTime,
Math::Random(m_LifeSpan.m_x, m_LifeSpan.m_y),
Math::fRandom(m_BeginSize.m_x, m_BeginSize.m_y) * particlesize * worldscale,
Math::fRandom(m_EndSize.m_x, m_EndSize.m_y) * particlesize * worldscale,
Math::fRandom(m_ColorPow.m_x, m_ColorPow.m_y),
Math::fRandom(m_AlphaPow.m_x, m_AlphaPow.m_y),
Math::fRandom(m_SizePow.m_x, m_SizePow.m_y)
);
m_Index++;
}
}
if( beginIndex + relaseNum > m_ArraySize ) //头尾事件，从新flush
{
int toend = m_ArraySize - beginIndex;
m_pVertexBuffer->FlushVertexBuffer( beginIndex, toend, &m_pParticle[beginIndex] );
m_pVertexBuffer->FlushVertexBuffer( 0, relaseNum - toend, &m_pParticle[0] );
}
else
{
m_pVertexBuffer->FlushVertexBuffer( beginIndex, relaseNum, &m_pParticle[beginIndex] );
}
}
}
}
}
}
}
std::vector<double> benchmark(size_t point_num)
{
// first generating the original input
// we generate all the types first
sensor_msgs::PointCloud2 ros_cloud;
pcl::PointCloud<OriginalPointType> pcl_cloud;
pcl::PCLPointCloud2 pcl2_cloud;
for (size_t i = 0; i < point_num; i++) {
OriginalPointType p;
pcl_cloud.points.push_back(p); // empty is OK, I beleive...
}
pcl::toROSMsg(pcl_cloud, ros_cloud);
pcl::toPCLPointCloud2(pcl_cloud, pcl2_cloud);
const sensor_msgs::PointCloud2 ros_cloud_const = ros_cloud;
clock_t c1, c2;
// sensor_msgs::PointCloud2 -> pcl::PCLPointCloud2
c1 = clock();
for (size_t i = 0; i < test_num; i++) {
pcl::PCLPointCloud2 tmp;
pcl_conversions::toPCL(ros_cloud_const, tmp);
}
c2 = clock();
double ros_to_pcl2 = TO_SEC(c1, c2);
// pcl::PCLPointCloud2 -> pcl::PointCloud
c1 = clock();
for (size_t i = 0; i < test_num; i++) {
pcl::PointCloud<IntermediatePointType> tmp;
pcl::fromPCLPointCloud2(pcl2_cloud, tmp);
}
c2 = clock();
double pcl2_to_pcl = TO_SEC(c1, c2);
// pcl::PointCloud -> pcl::PCLPointCloud2
c1 = clock();
for (size_t i = 0; i < test_num; i++) {
pcl::PCLPointCloud2 tmp;
pcl::toPCLPointCloud2(pcl_cloud, tmp);
}
c2 = clock();
double pcl_to_pcl2 = TO_SEC(c1, c2);
// pcl::PCLPointCloud2 -> sensor_msgs::PointCloud2
c1 = clock();
for (size_t i = 0; i < test_num; i++) {
sensor_msgs::PointCloud2 tmp;
pcl_conversions::fromPCL(pcl2_cloud, tmp);
}
c2 = clock();
double pcl2_to_ros = TO_SEC(c1, c2);
// sensor_msgs::PointCloud2 -> pcl::PointCloud
c1 = clock();
for (size_t i = 0; i < test_num; i++) {
pcl::PointCloud<IntermediatePointType> tmp;
pcl::fromROSMsg(ros_cloud, tmp);
}
c2 = clock();
double ros_to_pcl = TO_SEC(c1, c2);
// pcl::PointCloud -> sensor_msgs::PointCloud2
c1 = clock();
for (size_t i = 0; i < test_num; i++) {
sensor_msgs::PointCloud2 tmp;
pcl::toROSMsg(pcl_cloud, tmp);
}
c2 = clock();
double pcl_to_ros = TO_SEC(c1, c2);
std::vector<double> ret;
ret.push_back(ros_to_pcl2);
ret.push_back(pcl2_to_pcl);
ret.push_back(pcl_to_pcl2);
ret.push_back(pcl2_to_ros);
ret.push_back(ros_to_pcl);
ret.push_back(pcl_to_ros);
return ret;
}
int main(int argc, char *argv[]){
boost_po::options_description options("Allowed options");
std::string trainfname;
std::string testfname;
int get_train_err = 0;
options.add_options()
("train", boost_po::value<std::string>(&trainfname)->required(), "train")
("test", boost_po::value<std::string>(&testfname)->required(), "test")
("terr", boost_po::value<int>(&get_train_err), "get train err");
boost_po::variables_map options_map;
boost_po::store(boost_po::parse_command_line(argc, argv, options), options_map);
boost_po::notify(options_map);
arma::mat X;
arma::icolvec Y;
clock_t t;
std::cout << "start loading " << trainfname << std::endl;
START_TIMER(t);
X.load(trainfname, arma::csv_ascii);
END_TIMER(t, t);
std::cout << "data matrix loaded, size: " << X.n_rows << ", " << X.n_cols << " time = " << TO_SEC(t) << " sec" << std::endl;
Y = arma::conv_to<arma::icolvec>::from(X.col(X.n_cols - 1));
X.shed_col(X.n_cols - 1);
convert_binary(Y);
const int32_t num_samples = X.n_rows;
const int32_t num_features = X.n_cols;
arma::colvec mu(num_features); //gaussian mean
arma::mat sigma(num_features, num_features); //gaussian covariance
double xi = 2;
double delta = 1;
//initialize gaussian parameters
mu.zeros();
sigma = sigma.eye()*COV_FAC;
std::cout << "start inverting sigma" << std::endl;
START_TIMER(t);
arma::mat sigma_inv = sigma.i();
END_TIMER(t, t);
std::cout << "finished inverting, time = " << TO_SEC(t) << " secs" << std::endl;
clock_t ts;
std::cout << "starts training" << std::endl;
START_TIMER(ts);
int iter_cnt = 0;
while(std::abs(delta) > CONVERGE_THRE){
arma::mat post_sigma_inv;
arma::mat post_sigma;
arma::colvec post_mu;
double xi_sqr;
double post_xi;
int32_t i;
for(i = 0; i < num_samples; i++){
std::cout << "sample = i = " << i << std::endl;
arma::colvec x = X.row(i).t();
std::cout << "x.print()" << std::endl;
x.print();
post_sigma_inv = get_post_sigma_inv(xi, x, sigma_inv);
post_sigma = post_sigma_inv.i();
post_mu = get_post_mu(x, sigma_inv, post_sigma, mu, Y(i));
xi_sqr = get_xi_sqr(x, post_sigma, post_mu);
post_xi = sqrt(xi_sqr);
sigma = post_sigma;
sigma_inv = post_sigma_inv;
mu = post_mu;
delta = post_xi - xi;
xi = post_xi;
END_TIMER(t, ts);
std::cout << "xi = " << xi << " delta = " << delta << " time = " << TO_SEC(t) << " sec" << std::endl;
}
++iter_cnt;
std::cout << "iteration " << iter_cnt << " done. xi = " << xi << " delta = " << delta << " std::abs(delta) = " << std::abs(delta) << " time = " << TO_SEC(t) << " sec" << std::endl;
}
END_TIMER(t, ts);
std::cout << "time = " << TO_SEC(t) << " sec" << std::endl;
std::cout << " training done" << std::endl;
std::cout << "xi = " << xi << std::endl;
arma::mat X_test;
arma::icolvec Y_test;
std::cout << "start loading test matrix " << testfname << std::endl;
START_TIMER(t);
X_test.load(testfname, arma::csv_ascii);
END_TIMER(t, t);
std::cout << "test data matrix loaded, size: " << X_test.n_rows << ", " << X_test.n_cols << " time = " << TO_SEC(t) << " sec" << std::endl;
Y_test = arma::conv_to<arma::icolvec>::from(X_test.col(X_test.n_cols - 1));
X_test.shed_col(X_test.n_cols - 1);
double test_err = classification_error(xi, sigma, sigma_inv, mu, X_test, Y_test);
std::cout << "test error = " << test_err << std::endl;
if(get_train_err){
double train_err = classification_error(xi, sigma, sigma_inv, mu, X, Y);
std::cout << "train error = " << train_err << std::endl;
}
sigma.save(trainfname + ".sigma.out", arma::csv_ascii);
mu.save(trainfname + ".mu.out", arma::csv_ascii);
X.save(trainfname + ".out", arma::csv_ascii);
}
static inline void latency_out(FILE *ofp, __u64 tstamp, __u64 latency)
{
if (ofp)
fprintf(ofp, "%lf %lf\n", TO_SEC(tstamp), TO_SEC(latency));
}
