/*
compute derivative for each particle in particles1 with respect to particles2
SCORING function : chi
For calculation in real space the quantity Delta(r) is needed to get
derivatives of an atom
Delta(r) = f_iatom * sum_i f_i delta(r-r_{i,iatom}) (x_iatom-x_i)
// e_q = exp( -0.23 * q*q )
*/
void DerivativeCalculator::compute_chisquare_derivative(
const Profile* model_profile, const Particles& particles1,
const Particles& particles2,
Vector<algebra::Vector3D>& derivatives,
const Vector<double>& effect_size) const {
algebra::Vector3D dIdx, chisquare_derivative;
Vector<Vector<double> > sinc_cos_values;
DeltaDistributionFunction delta_dist = precompute_derivative_helpers(
model_profile, particles1, particles2, sinc_cos_values);
unsigned int profile_size =
std::min(model_profile->size(), exp_profile_->size());
derivatives.clear();
derivatives.resize(particles1.size());
for (unsigned int iatom = 0; iatom < particles1.size(); iatom++) {
// Compute a delta distribution per atom
delta_dist.calculate_derivative_distribution(particles1[iatom]);
chisquare_derivative = algebra::Vector3D(0.0, 0.0, 0.0);
for (unsigned int iq = 0; iq < profile_size; iq++) {
compute_intensity_derivatives(delta_dist, sinc_cos_values, iq, dIdx);
chisquare_derivative += dIdx * effect_size[iq];
}
derivatives[iatom] = chisquare_derivative;
}
}
IMPEM_BEGIN_NAMESPACE
EnvelopePenetrationRestraint::EnvelopePenetrationRestraint(
Particles ps,
DensityMap *em_map,Float threshold
): Restraint("Envelope penetration restraint")
{
IMP_LOG_TERSE("Load envelope penetration with the following input:"<<
"number of particles:"<<ps.size()<<
"\n");
threshold_=threshold;
target_dens_map_ = em_map;
IMP_IF_CHECK(USAGE) {
for (unsigned int i=0; i< ps.size(); ++i) {
IMP_USAGE_CHECK(core::XYZR::particle_is_instance(ps[i]),
"Particle " << ps[i]->get_name()
<< " is not XYZR"
<< std::endl);
}
}
add_particles(ps);
ps_=ps;
IMP_LOG_TERSE("after adding particles"<<std::endl);
IMP_LOG_TERSE( "Finish initialization" << std::endl);
}
/*
* loop over all particles and rigid bodies, and call compute_chi_derivative on
* them
*/
void DerivativeCalculator::compute_all_derivatives(const Particles& particles,
const std::vector<Particles>& rigid_bodies,
const std::vector<core::RigidBody>& rigid_bodies_decorators,
const Profile& model_profile,
const std::vector<double>& effect_size,
DerivativeAccumulator *acc) const
{
std::vector<IMP::algebra::VectorD<3> > derivatives;
const FloatKeys keys = IMP::core::XYZ::get_xyz_keys();
// 1. compute derivatives for each rigid body
for(unsigned int i=0; i<rigid_bodies.size(); i++) {
if(!rigid_bodies_decorators[i].get_coordinates_are_optimized()) continue;
// contribution from other rigid bodies
for(unsigned int j=0; j<rigid_bodies.size(); j++) {
if(i == j) continue;
compute_chisquare_derivative(model_profile, rigid_bodies[i],
rigid_bodies[j], derivatives, effect_size);
for (unsigned int k = 0; k < rigid_bodies[i].size(); k++) {
rigid_bodies[i][k]->add_to_derivative(keys[0],derivatives[k][0], *acc);
rigid_bodies[i][k]->add_to_derivative(keys[1],derivatives[k][1], *acc);
rigid_bodies[i][k]->add_to_derivative(keys[2],derivatives[k][2], *acc);
}
}
if(particles.size() > 0) {
// contribution from other particles
compute_chisquare_derivative(model_profile, rigid_bodies[i],
particles, derivatives, effect_size);
for (unsigned int k = 0; k < rigid_bodies[i].size(); k++) {
rigid_bodies[i][k]->add_to_derivative(keys[0],derivatives[k][0], *acc);
rigid_bodies[i][k]->add_to_derivative(keys[1],derivatives[k][1], *acc);
rigid_bodies[i][k]->add_to_derivative(keys[2],derivatives[k][2], *acc);
}
}
}
// 2. compute derivatives for other particles
if(particles.size() > 0) {
// particles own contribution
compute_chisquare_derivative(model_profile, particles, derivatives,
effect_size);
for (unsigned int i = 0; i < particles.size(); i++) {
particles[i]->add_to_derivative(keys[0], derivatives[i][0], *acc);
particles[i]->add_to_derivative(keys[1], derivatives[i][1], *acc);
particles[i]->add_to_derivative(keys[2], derivatives[i][2], *acc);
}
// rigid bodies contribution
for(unsigned int i=0; i<rigid_bodies.size(); i++) {
compute_chisquare_derivative(model_profile, particles, rigid_bodies[i],
derivatives, effect_size);
for (unsigned int i = 0; i < particles.size(); i++) {
particles[i]->add_to_derivative(keys[0], derivatives[i][0], *acc);
particles[i]->add_to_derivative(keys[1], derivatives[i][1], *acc);
particles[i]->add_to_derivative(keys[2], derivatives[i][2], *acc);
}
}
}
}
const Particles &_give_particles(Model *m) {
static Particles ret;
while (ret.size() < 10) {
ret.push_back(new Particle(m));
}
return ret;
}
void AnchorsData::set_secondary_structure_probabilities(
const Particles &ssres_ps, const Ints &indices) {
IMP_USAGE_CHECK(secondary_structure_ps_.size() == points_.size(),
"Secondary structure has not been set up, "
"run AnchorsData::setup_secondary_structure() first");
int anum;
for (int ssnum = 0; ssnum < (int)ssres_ps.size(); ssnum++) {
IMP_USAGE_CHECK(
atom::SecondaryStructureResidue::get_is_setup(ssres_ps[ssnum]),
"SSE Particles must be decorated as"
"SecondaryStructureResidues");
if (indices.size() == 0)
anum = ssnum;
else
anum = indices[ssnum];
atom::SecondaryStructureResidue(secondary_structure_ps_[anum])
.set_prob_helix(
atom::SecondaryStructureResidue(ssres_ps[ssnum]).get_prob_helix());
atom::SecondaryStructureResidue(secondary_structure_ps_[anum])
.set_prob_strand(atom::SecondaryStructureResidue(ssres_ps[ssnum])
.get_prob_strand());
atom::SecondaryStructureResidue(secondary_structure_ps_[anum])
.set_prob_coil(
atom::SecondaryStructureResidue(ssres_ps[ssnum]).get_prob_coil());
}
}
Particles _give_particles_copy(Model *m) {
Particles ret;
while (ret.size() < 10) {
ret.push_back(new Particle(m));
}
return ret;
}
Particles RigidBodyMover::get_particles(Particles ps) {
Particles ps_norb;
for (unsigned i = 0; i < ps.size(); ++i) {
if (!core::RigidMember::get_is_setup(ps[i])) {
ps_norb.push_back(ps[i]);
}
}
return ps_norb;
}
RadiusOfGyrationRestraint::RadiusOfGyrationRestraint(Particles ps,
int num_residues,
Float scale):
Restraint(IMP::internal::get_model(ps), "RadiusOfGyrationRestraint"){
if (ps.size()==0) return;
add_particles(ps);
mdl_=ps[0]->get_model();
predicted_rog_ = get_approximated_radius_of_gyration(num_residues);
scale_=scale;
hub_=new core::HarmonicUpperBound(predicted_rog_*scale_,1);
}
virtual void onInitParticles(Particles& particles)
{
const float time = parentDocument()->getAnimationTime();
int oldNb = birth.size();
int newNb = oldNb + particles.size();
birth.resize(newNb);
std::uniform_real_distribution<float> dist(0, variation.getValue());
for(int i=oldNb; i<newNb; ++i)
birth[i] = time - dist(gen); // Giving them negative age based on the variation value
}
void add_PbcBoxedMover(Particles ps, double dx, algebra::Vector3Ds centers,
algebra::Transformation3Ds trs,
core::MonteCarloMovers &mvs, Particle *SideXY,
Particle *SideZ) {
IMP_NEW(spb::PbcBoxedMover, mv,
(ps[0], ps, dx, centers, trs, SideXY, SideXY, SideZ));
mvs.push_back(mv);
for (unsigned int k = 1; k < ps.size(); ++k) {
IMP_NEW(core::BallMover, bmv, (ps[k]->get_model(), ps[k]->get_index(), dx));
mvs.push_back(bmv);
}
}
void randomizeInSphere(Particles& particles, real sphereSize)
{
const unsigned size = particles.size();
//const Real3& spherePos = sphere.p_;
particles.pos_.resize(size);
for (unsigned i=0;i<size;++i) {
particles.pos_[i]= sphereSize*( Real3(-.5f,-.5f,-.5f)+Real3(randnorm<real>(),randnorm<real>(),randnorm<real>()) );
// particles.pos_[i]= i%2?Real3(sphereSize-0.1,0,0):Real3(-sphereSize+0.1,0,0);
// particles.vel_[i]= i%2?Real3(2.5,0.5,0):Real3(-3,0,0);
// particles.acc_[i]=Real3(0.f,0.f,0.f);
}
}
TALOSRestraint::TALOSRestraint(Particles p, unsigned N, double R0, double
chiexp, Particle *kappa) : kappa_(kappa) {
if (p.size() != 4) {
IMP_THROW("please provide a list with 4 particles!", ModelException);
}
p_[0]=static_cast<Particle*>(p[0]);
p_[1]=static_cast<Particle*>(p[1]);
p_[2]=static_cast<Particle*>(p[2]);
p_[3]=static_cast<Particle*>(p[3]);
// create von Mises
double kappaval=Scale(kappa_).get_scale();
mises_ = new vonMisesSufficient(0, N, R0, chiexp, kappaval);
//mises_->set_was_used(true);
}
//LC: added for local (re)localization
void MapModel::distributeLocally(Particles& particles, double roll, double pitch, double yaw,
double x, double y, double z,
UniformGeneratorT& rngUniform, double dist_linear, double dist_angular){ //maxHeight added by LC
double sizeX,sizeY,sizeZ, minX, minY, minZ;
//LC: get total map size
m_map->getMetricSize(sizeX,sizeY,sizeZ);
//LC: get minimum values of map bounding box
m_map->getMetricMin(minX, minY, minZ);
double weight = 1.0 / particles.size();
Particles::iterator it = particles.begin();
while (true){
if (it == particles.end())
break;
// obtain a pose hypothesis:
double x_new = x + dist_linear * ((rngUniform() - 0.5) * 2);
double y_new = y + dist_linear * ((rngUniform() - 0.5) * 2);
double z_new = z + dist_linear * ((rngUniform() - 0.5) * 2);
double roll_new = roll;// + dist_angular * ((rngUniform() - 0.5) * 2);
double pitch_new = pitch;// + dist_angular * ((rngUniform() - 0.5) * 2);
double yaw_new = yaw + dist_angular * ((rngUniform() - 0.5) * 2);
if(it == particles.begin()) {
// obtain a pose hypothesis:
x_new = x;
y_new = y;
z_new = z;
roll_new = roll;
pitch_new = pitch;
yaw_new = yaw;
}
//set new pose
it->pose.getOrigin().setX(x_new);
it->pose.getOrigin().setY(y_new);
it->pose.getOrigin().setZ(z_new);
//set orientation
it->pose.setRotation(tf::createQuaternionFromRPY(roll_new, pitch_new, yaw_new));
it->weight = weight;
it++;
}
}
/** @brief get the central pixel brightess for each coordinate in centers. */
vector<float> Tracker::getIntensities(const Particles ¢ers)
{
vector<float> ret(centers.size());
for(size_t p=0; p<centers.size(); ++p)
{
//convert real coordinates to integer strides
boost::array<size_t, 3> tmp;
for(size_t d=0; d<tmp.size(); ++d)
tmp[d] = centersMap.strides()[d] * (size_t)centers[p][(fortran_order)?(2-d):d];
//normlize the value by the size of the image to get rid of the FTTW unnormalization and be in the same unit as the threshold parameter
ret[p] = (*(data+accumulate(tmp.begin(), tmp.end(), 0)))/centersMap.num_elements();
}
return ret;
}
std::vector<core::RigidBody> RigidBodyMover::get_rigid_bodies(
Particles ps) {
std::vector<core::RigidBody> rbs;
for (unsigned i = 0; i < ps.size(); ++i) {
if (core::RigidMember::get_is_setup(ps[i])) {
core::RigidBody rb = core::RigidMember(ps[i]).get_rigid_body();
std::vector<core::RigidBody>::iterator it =
find(rbs.begin(), rbs.end(), rb);
if (it == rbs.end()) {
rbs.push_back(rb);
}
}
}
return rbs;
}
display::Geometries get_rigid_body_derivative_geometries(
Model *m, ParticleIndex pi) {
RigidBody d(m, pi);
display::Geometries ret;
Particles ms = get_as<Particles>(d.get_rigid_members());
algebra::Transformation3D otr =
d.get_reference_frame().get_transformation_to();
algebra::VectorD<4> rderiv = d.get_rotational_derivatives();
algebra::Vector3D tderiv = d.get_derivatives();
algebra::VectorD<4> rot = otr.get_rotation().get_quaternion();
IMP_LOG_TERSE("Old rotation was " << rot << std::endl);
Float scale = .1;
algebra::VectorD<4> dv = rderiv;
if (dv.get_squared_magnitude() > 0.00001) {
dv = scale * dv.get_unit_vector();
}
rot += dv;
rot = rot.get_unit_vector();
algebra::Rotation3D r(rot[0], rot[1], rot[2], rot[3]);
IMP_LOG_TERSE("Derivative was " << tderiv << std::endl);
IMP_LOG_TERSE("New rotation is " << rot << std::endl);
FloatRange xr =
d.get_particle()->get_model()->get_range(core::XYZ::get_xyz_keys()[0]);
Float wid = xr.second - xr.first;
algebra::Vector3D stderiv = scale * tderiv * wid;
algebra::Transformation3D ntr(
algebra::Rotation3D(rot[0], rot[1], rot[2], rot[3]),
stderiv + otr.get_translation());
for (unsigned int i = 0; i < ms.size(); ++i) {
core::RigidMember dm(ms[i]);
display::SegmentGeometry *tr = new display::SegmentGeometry(
algebra::Segment3D(dm.get_coordinates(), dm.get_coordinates() + tderiv),
/*xyzcolor_*/
display::Color(1, 0, 0));
ret.push_back(tr);
algebra::Vector3D ic =
r.get_rotated(dm.get_internal_coordinates()) + d.get_coordinates();
display::SegmentGeometry *rtr = new display::SegmentGeometry(
algebra::Segment3D(dm.get_coordinates(), ic), display::Color(0, 1, 0));
ret.push_back(rtr);
display::SegmentGeometry *nrtr = new display::SegmentGeometry(
algebra::Segment3D(dm.get_coordinates(),
ntr.get_transformed(dm.get_internal_coordinates())),
display::Color(0, 0, 1));
ret.push_back(nrtr);
}
return ret;
}
IMPISD_BEGIN_NAMESPACE
TALOSRestraint::TALOSRestraint(Particles p, Floats data, Particle *kappa)
: kappa_(kappa) {
if (p.size() != 4) {
IMP_THROW("please provide a list with 4 particles!", ModelException);
}
p_[0]=static_cast<Particle*>(p[0]);
p_[1]=static_cast<Particle*>(p[1]);
p_[2]=static_cast<Particle*>(p[2]);
p_[3]=static_cast<Particle*>(p[3]);
// create von Mises
double kappaval=Scale(kappa_).get_scale();
mises_ = new vonMisesSufficient(0, data, kappaval);
//mises_->set_was_used(true);
}
void Interpolator1D3Order::fieldsAndCurrents( ElectroMagn *EMfields, Particles &particles, SmileiMPI *smpi, int *istart, int *iend, int ithread, LocalFields *JLoc, double *RhoLoc )
{
int ipart = *istart;
double *ELoc = &( smpi->dynamics_Epart[ithread][ipart] );
double *BLoc = &( smpi->dynamics_Bpart[ithread][ipart] );
//!\todo Julien, can you check that this is indeed the centered B-field which is passed to the pusher?
Field1D *Ex1D = static_cast<Field1D *>( EMfields->Ex_ );
Field1D *Ey1D = static_cast<Field1D *>( EMfields->Ey_ );
Field1D *Ez1D = static_cast<Field1D *>( EMfields->Ez_ );
Field1D *Bx1D_m = static_cast<Field1D *>( EMfields->Bx_m );
Field1D *By1D_m = static_cast<Field1D *>( EMfields->By_m );
Field1D *Bz1D_m = static_cast<Field1D *>( EMfields->Bz_m );
Field1D *Jx1D = static_cast<Field1D *>( EMfields->Jx_ );
Field1D *Jy1D = static_cast<Field1D *>( EMfields->Jy_ );
Field1D *Jz1D = static_cast<Field1D *>( EMfields->Jz_ );
Field1D *Rho1D = static_cast<Field1D *>( EMfields->rho_ );
// Calculate the normalized positions
double xjn = particles.position( 0, ipart )*dx_inv_;
// Calculate coeffs
coeffs( xjn );
int nparts( particles.size() );
// Interpolate the fields from the Dual grid : Ex, By, Bz
*( ELoc+0*nparts ) = compute( coeffd_, Ex1D, id_ );
*( BLoc+1*nparts ) = compute( coeffd_, By1D_m, id_ );
*( BLoc+2*nparts ) = compute( coeffd_, Bz1D_m, id_ );
// Interpolate the fields from the Primal grid : Ey, Ez, Bx
*( ELoc+1*nparts ) = compute( coeffp_, Ey1D, ip_ );
*( ELoc+2*nparts ) = compute( coeffp_, Ez1D, ip_ );
*( BLoc+0*nparts ) = compute( coeffp_, Bx1D_m, ip_ );
// Primal Grid : Jy, Jz, Rho
JLoc->y = compute( coeffp_, Jy1D, ip_ );
JLoc->z = compute( coeffp_, Jz1D, ip_ );
( *RhoLoc ) = compute( coeffp_, Rho1D, ip_ );
// Dual Grid : Jx
JLoc->x = compute( coeffd_, Jx1D, id_ );
}
// Interpolator specific to tracked particles. A selection of particles may be provided
void Interpolator1D3Order::fieldsSelection( ElectroMagn *EMfields, Particles &particles, double *buffer, int offset, vector<unsigned int> *selection )
{
if( selection ) {
int nsel_tot = selection->size();
for( int isel=0 ; isel<nsel_tot; isel++ ) {
fields( EMfields, particles, ( *selection )[isel], offset, buffer+isel, buffer+isel+3*offset );
}
} else {
int npart_tot = particles.size();
for( int ipart=0 ; ipart<npart_tot; ipart++ ) {
fields( EMfields, particles, ipart, offset, buffer+ipart, buffer+ipart+3*offset );
}
}
}
void update_cells_parallel(int start_row, int end_row, Particles &my_particles, CellMatrix &cells)
{
for(int i = 0; i < my_particles.size(); i++)
{
particle_t *curr_particle = my_particles[i];
Point p = get_cell_index(*curr_particle);
if(p.y >= end_row || p.y < start_row)
{
pthread_mutex_lock(&cs);
cells[p.y][p.x].push_back(curr_particle);
pthread_mutex_unlock(&cs);
}
else
{
cells[p.y][p.x].push_back(curr_particle);
}
}
}
void Interpolator1D3Order::fieldsWrapper( ElectroMagn *EMfields, Particles &particles, SmileiMPI *smpi, int *istart, int *iend, int ithread, int ipart_ref )
{
std::vector<double> *Epart = &( smpi->dynamics_Epart[ithread] );
std::vector<double> *Bpart = &( smpi->dynamics_Bpart[ithread] );
std::vector<int> *iold = &( smpi->dynamics_iold[ithread] );
std::vector<double> *delta = &( smpi->dynamics_deltaold[ithread] );
//Loop on bin particles
int npart_tot = particles.size();
for( int ipart=*istart ; ipart<*iend; ipart++ ) {
//Interpolation on current particle
fields( EMfields, particles, ipart, npart_tot, &( *Epart )[ipart], &( *Bpart )[ipart] );
//Buffering of iol and delta
( *iold )[ipart] = ip_;
( *delta )[ipart] = xi;
}
}
void MapModel::initGlobal(Particles& particles, double z, double roll, double pitch,
const Vector6d& initNoise,
UniformGeneratorT& rngUniform, NormalGeneratorT& rngNormal, double maxHeight, double minHeight){ //maxHeight added by LC
double sizeX,sizeY,sizeZ, minX, minY, minZ;
//LC: get total map size
m_map->getMetricSize(sizeX,sizeY,sizeZ);
//LC: get minimum values of map bounding box
m_map->getMetricMin(minX, minY, minZ);
double weight = 1.0 / particles.size();
Particles::iterator it = particles.begin();
while (true){
if (it == particles.end())
break;
// obtain a pose hypothesis:
double x = minX + sizeX * rngUniform();
double y = minY + sizeY * rngUniform();
std::vector<double> z_list;
getHeightlist(x, y, minHeight, maxHeight,z_list);
//LC: create a pose with random yaw orientation at (x,y) for every entry in the height list
for (unsigned zIdx = 0; zIdx < z_list.size(); zIdx++){
if (it == particles.end())
break;
// not needed => we already know that z contains valid poses
// distance map: used distance from obstacles:
//std::abs(node->getLogOdds()) < 0.1){
// if (!isOccupied(octomap::point3d(x, y, z[zIdx]))){
it->pose.getOrigin().setX(x);
it->pose.getOrigin().setY(y);
// TODO: sample z, roll, pitch
it->pose.getOrigin().setZ(z_list.at(zIdx) + z + rngNormal() * initNoise(2));
//LC: create yaw orientation from -PI to +PI
double yaw = rngUniform() * 2 * M_PI -M_PI;
it->pose.setRotation(tf::createQuaternionFromRPY(roll, pitch, yaw));
it->weight = weight;
it++;
}
}
}
void ObservationModel::integratePoseMeasurement(Particles& particles, double poseRoll, double posePitch, const tf::StampedTransform& footprintToTorso){
double poseHeight = footprintToTorso.getOrigin().getZ();
ROS_DEBUG("Pose measurement z=%f R=%f P=%f", poseHeight, poseRoll, posePitch);
// TODO cluster xy of particles => speedup
#pragma omp parallel for
for (unsigned i=0; i < particles.size(); ++i){
// integrate IMU meas.:
double roll, pitch, yaw;
particles[i].pose.getBasis().getRPY(roll, pitch, yaw);
particles[i].weight += m_weightRoll * logLikelihood(poseRoll - roll, m_sigmaRoll);
particles[i].weight += m_weightPitch * logLikelihood(posePitch - pitch, m_sigmaPitch);
// integrate height measurement (z)
double heightError = 0;
// if (getHeightError(particles[i],footprintToTorso, heightError))
// particles[i].weight += m_weightZ * logLikelihood(heightError, m_sigmaZ);
particles[i].weight += m_weightZ * logLikelihood(heightError, m_sigmaZ);
}
}
SecondaryStructureResidues setup_coarse_secondary_structure_residues(
const Particles &ssr_ps, Model *mdl, int coarse_factor,
int start_res_num, bool winner_takes_all_per_res) {
/* We're presuming that the coarsening starts from 0.
So if start_res_num%coarse_factor<coarse_factor/2, this
set starts with the majority of a node.
Likewise if start_res_num%coarse_factor>=coarse_factor/2, this
set starts with the minority of a node, so we ignore the first few
particles.
Same thing for the end! If has a little tail, ignore last few residues.
*/
SecondaryStructureResidues ssrs;
int start_idx = 0, stop_idx = (int)ssr_ps.size();
if (start_res_num % coarse_factor >= float(coarse_factor) / 2) {
start_idx = coarse_factor - start_res_num % coarse_factor;
}
if ((stop_idx + start_res_num) % coarse_factor < float(coarse_factor) / 2) {
stop_idx -= (stop_idx + start_res_num) % coarse_factor;
}
// now start grouping
int prev_coarse = (start_idx + start_res_num) / coarse_factor;
Particles tmp_ps;
for (int nr = start_idx; nr < stop_idx; nr++) {
int this_coarse = (nr + start_res_num) / coarse_factor;
if (this_coarse != prev_coarse) {
ssrs.push_back(setup_coarse_secondary_structure_residue(
tmp_ps, mdl, winner_takes_all_per_res));
tmp_ps.clear();
}
tmp_ps.push_back(ssr_ps[nr]);
prev_coarse = this_coarse;
}
if (tmp_ps.size() > 0) {
ssrs.push_back(setup_coarse_secondary_structure_residue(
tmp_ps, mdl, winner_takes_all_per_res));
}
return ssrs;
}
void MapModel::initGlobal(Particles& particles,
const Vector6d& initPose, const Vector6d& initNoise,
UniformGeneratorT& rngUniform, NormalGeneratorT& rngNormal){
double sizeX,sizeY,sizeZ, minX, minY, minZ;
m_map->getMetricSize(sizeX,sizeY,sizeZ);
m_map->getMetricMin(minX, minY, minZ);
double weight = 1.0 / particles.size();
Particles::iterator it = particles.begin();
while (true){
if (it == particles.end())
break;
// obtain a pose hypothesis:
double x = minX + sizeX * rngUniform();
double y = minY + sizeY * rngUniform();
std::vector<double> z;
getHeightlist(x, y, 0.6,z);
for (unsigned zIdx = 0; zIdx < z.size(); zIdx++){
if (it == particles.end())
break;
// not needed => we already know that z contains valid poses
// distance map: used distance from obstacles:
//std::abs(node->getLogOdds()) < 0.1){
// if (!isOccupied(octomap::point3d(x, y, z[zIdx]))){
it->pose.getOrigin().setX(x);
it->pose.getOrigin().setY(y);
// TODO: sample z, roll, pitch around odom pose!
it->pose.getOrigin().setZ(z.at(zIdx) + initPose(2) + rngNormal() * initNoise(2));
double yaw = rngUniform() * 2 * M_PI -M_PI;
it->pose.setRotation(tf::createQuaternionFromYaw(yaw));
it->weight = weight;
it++;
}
}
}
void add_BallMover(Particles ps, double dx, core::MonteCarloMovers &mvs) {
for (unsigned int k = 0; k < ps.size(); ++k) {
IMP_NEW(core::BallMover, bmv, (ps[k]->get_model(), ps[k]->get_index(), dx));
mvs.push_back(bmv);
}
}
algebra::Vector3Ds CoarseCC::calc_derivatives(const DensityMap *em_map,
const DensityMap *model_map,
const Particles &model_ps,
const FloatKey &w_key,
KernelParameters *kernel_params,
const float &scalefac,
const algebra::Vector3Ds &dv) {
algebra::Vector3Ds dv_out;
dv_out.insert(dv_out.end(), dv.size(), algebra::Vector3D(0., 0., 0.));
double tdvx = 0., tdvy = 0., tdvz = 0., tmp, rsq;
int iminx, iminy, iminz, imaxx, imaxy, imaxz;
const DensityHeader *model_header = model_map->get_header();
const DensityHeader *em_header = em_map->get_header();
const float *x_loc = em_map->get_x_loc();
const float *y_loc = em_map->get_y_loc();
const float *z_loc = em_map->get_z_loc();
IMP_INTERNAL_CHECK(model_ps.size() == dv.size(),
"input derivatives array size does not match "
<< "the number of particles in the model map\n");
core::XYZRs model_xyzr = core::XYZRs(model_ps);
// this would go away once we have XYZRW decorator
const emreal *em_data = em_map->get_data();
float lim = kernel_params->get_lim();
long ivox;
// validate that the model and em maps are not empty
IMP_USAGE_CHECK(em_header->rms >= EPS,
"EM map is empty ! em_header->rms = " << em_header->rms);
// it may be that CG takes a too large step, which causes the particles
// to go outside of the density
// if (model_header->rms <= EPS){
// IMP_WARN("Model map is empty ! model_header->rms = " << model_header->rms
// <<" derivatives are not calculated. the model centroid is : " <<
// core::get_centroid(core::XYZs(model_ps))<<
// " the map centroid is " << em_map->get_centroid()<<
// "number of particles in model:"<<model_ps.size()
//<<std::endl);
// return;
// }
// Compute the derivatives
int nx = em_header->get_nx();
int ny = em_header->get_ny();
// int nz=em_header->get_nz();
IMP_INTERNAL_CHECK(em_map->get_rms_calculated(),
"RMS should be calculated for calculating derivatives \n");
long nvox = em_header->get_number_of_voxels();
double lower_comp = 1. * nvox * em_header->rms * model_header->rms;
for (unsigned int ii = 0; ii < model_ps.size(); ii++) {
float x, y, z;
x = model_xyzr[ii].get_x();
y = model_xyzr[ii].get_y();
z = model_xyzr[ii].get_z();
IMP_IF_LOG(VERBOSE) {
algebra::Vector3D vv(x, y, z);
IMP_LOG_VERBOSE(
"start value:: (" << x << "," << y << "," << z << " ) "
<< em_map->get_value(x, y, z) << " : "
<< em_map->get_dim_index_by_location(vv, 0) << ","
<< em_map->get_dim_index_by_location(vv, 1) << ","
<< em_map->get_dim_index_by_location(vv, 2)
<< std::endl);
}
const RadiusDependentKernelParameters ¶ms =
kernel_params->get_params(model_xyzr[ii].get_radius());
calc_local_bounding_box( // em_map,
model_map, x, y, z, params.get_kdist(), iminx, iminy, iminz, imaxx,
imaxy, imaxz);
IMP_LOG_WRITE(VERBOSE, params.show());
IMP_LOG_VERBOSE("local bb: [" << iminx << "," << iminy << "," << iminz
<< "] [" << imaxx << "," << imaxy << ","
<< imaxz << "] \n");
tdvx = .0;
tdvy = .0;
tdvz = .0;
for (int ivoxz = iminz; ivoxz <= imaxz; ivoxz++) {
for (int ivoxy = iminy; ivoxy <= imaxy; ivoxy++) {
ivox = ivoxz * nx * ny + ivoxy * nx + iminx;
for (int ivoxx = iminx; ivoxx <= imaxx; ivoxx++) {
/* if (em_data[ivox]<EPS) {
ivox++;
continue;
}*/
float dx = x_loc[ivox] - x;
float dy = y_loc[ivox] - y;
float dz = z_loc[ivox] - z;
rsq = dx * dx + dy * dy + dz * dz;
rsq = EXP(-rsq * params.get_inv_sigsq());
tmp = (x - x_loc[ivox]) * rsq;
if (std::abs(tmp) > lim) {
tdvx += tmp * em_data[ivox];
}
tmp = (y - y_loc[ivox]) * rsq;
if (std::abs(tmp) > lim) {
tdvy += tmp * em_data[ivox];
}
tmp = (z - z_loc[ivox]) * rsq;
if (std::abs(tmp) > lim) {
tdvz += tmp * em_data[ivox];
}
ivox++;
}
}
}
tmp = model_ps[ii]->get_value(w_key) * 2. * params.get_inv_sigsq() *
scalefac * params.get_normfac() / lower_comp;
IMP_LOG_VERBOSE("for particle:" << ii << " (" << tdvx << "," << tdvy << ","
<< tdvz << ")" << std::endl);
dv_out[ii][0] = tdvx * tmp;
dv_out[ii][1] = tdvy * tmp;
dv_out[ii][2] = tdvz * tmp;
} // particles
return dv_out;
}
// ---------------------------------------------------------------------------------------------------------------------
//! Project current densities : main projector
// ---------------------------------------------------------------------------------------------------------------------
void Projector2D4Order::currents( double *Jx, double *Jy, double *Jz, Particles &particles, unsigned int ipart, double invgf, int *iold, double *deltaold )
{
int nparts = particles.size();
// -------------------------------------
// Variable declaration & initialization
// -------------------------------------
int iloc;
// (x,y,z) components of the current density for the macro-particle
double charge_weight = inv_cell_volume * ( double )( particles.charge( ipart ) )*particles.weight( ipart );
double crx_p = charge_weight*dx_ov_dt;
double cry_p = charge_weight*dy_ov_dt;
double crz_p = charge_weight*one_third*particles.momentum( 2, ipart )*invgf;
// variable declaration
double xpn, ypn;
double delta, delta2, delta3, delta4;
// arrays used for the Esirkepov projection method
double Sx0[7], Sx1[7], Sy0[7], Sy1[7], DSx[7], DSy[7], tmpJx[7];
for( unsigned int i=0; i<7; i++ ) {
Sx1[i] = 0.;
Sy1[i] = 0.;
tmpJx[i] = 0.;
}
Sx0[0] = 0.;
Sx0[6] = 0.;
Sy0[0] = 0.;
Sy0[6] = 0.;
// --------------------------------------------------------
// Locate particles & Calculate Esirkepov coef. S, DS and W
// --------------------------------------------------------
// locate the particle on the primal grid at former time-step & calculate coeff. S0
delta = deltaold[0*nparts];
delta2 = delta*delta;
delta3 = delta2*delta;
delta4 = delta3*delta;
Sx0[1] = dble_1_ov_384 - dble_1_ov_48 * delta + dble_1_ov_16 * delta2 - dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
Sx0[2] = dble_19_ov_96 - dble_11_ov_24 * delta + dble_1_ov_4 * delta2 + dble_1_ov_6 * delta3 - dble_1_ov_6 * delta4;
Sx0[3] = dble_115_ov_192 - dble_5_ov_8 * delta2 + dble_1_ov_4 * delta4;
Sx0[4] = dble_19_ov_96 + dble_11_ov_24 * delta + dble_1_ov_4 * delta2 - dble_1_ov_6 * delta3 - dble_1_ov_6 * delta4;
Sx0[5] = dble_1_ov_384 + dble_1_ov_48 * delta + dble_1_ov_16 * delta2 + dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
delta = deltaold[1*nparts];
delta2 = delta*delta;
delta3 = delta2*delta;
delta4 = delta3*delta;
Sy0[1] = dble_1_ov_384 - dble_1_ov_48 * delta + dble_1_ov_16 * delta2 - dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
Sy0[2] = dble_19_ov_96 - dble_11_ov_24 * delta + dble_1_ov_4 * delta2 + dble_1_ov_6 * delta3 - dble_1_ov_6 * delta4;
Sy0[3] = dble_115_ov_192 - dble_5_ov_8 * delta2 + dble_1_ov_4 * delta4;
Sy0[4] = dble_19_ov_96 + dble_11_ov_24 * delta + dble_1_ov_4 * delta2 - dble_1_ov_6 * delta3 - dble_1_ov_6 * delta4;
Sy0[5] = dble_1_ov_384 + dble_1_ov_48 * delta + dble_1_ov_16 * delta2 + dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
// locate the particle on the primal grid at current time-step & calculate coeff. S1
xpn = particles.position( 0, ipart ) * dx_inv_;
int ip = round( xpn );
int ipo = iold[0*nparts];
int ip_m_ipo = ip-ipo-i_domain_begin;
delta = xpn - ( double )ip;
delta2 = delta*delta;
delta3 = delta2*delta;
delta4 = delta3*delta;
Sx1[ip_m_ipo+1] = dble_1_ov_384 - dble_1_ov_48 * delta + dble_1_ov_16 * delta2 - dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
Sx1[ip_m_ipo+2] = dble_19_ov_96 - dble_11_ov_24 * delta + dble_1_ov_4 * delta2 + dble_1_ov_6 * delta3 - dble_1_ov_6 * delta4;
Sx1[ip_m_ipo+3] = dble_115_ov_192 - dble_5_ov_8 * delta2 + dble_1_ov_4 * delta4;
Sx1[ip_m_ipo+4] = dble_19_ov_96 + dble_11_ov_24 * delta + dble_1_ov_4 * delta2 - dble_1_ov_6 * delta3 - dble_1_ov_6 * delta4;
Sx1[ip_m_ipo+5] = dble_1_ov_384 + dble_1_ov_48 * delta + dble_1_ov_16 * delta2 + dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
ypn = particles.position( 1, ipart ) * dy_inv_;
int jp = round( ypn );
int jpo = iold[1*nparts];
int jp_m_jpo = jp-jpo-j_domain_begin;
delta = ypn - ( double )jp;
delta2 = delta*delta;
delta3 = delta2*delta;
delta4 = delta3*delta;
Sy1[jp_m_jpo+1] = dble_1_ov_384 - dble_1_ov_48 * delta + dble_1_ov_16 * delta2 - dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
Sy1[jp_m_jpo+2] = dble_19_ov_96 - dble_11_ov_24 * delta + dble_1_ov_4 * delta2 + dble_1_ov_6 * delta3 - dble_1_ov_6 * delta4;
Sy1[jp_m_jpo+3] = dble_115_ov_192 - dble_5_ov_8 * delta2 + dble_1_ov_4 * delta4;
Sy1[jp_m_jpo+4] = dble_19_ov_96 + dble_11_ov_24 * delta + dble_1_ov_4 * delta2 - dble_1_ov_6 * delta3 - dble_1_ov_6 * delta4;
Sy1[jp_m_jpo+5] = dble_1_ov_384 + dble_1_ov_48 * delta + dble_1_ov_16 * delta2 + dble_1_ov_12 * delta3 + dble_1_ov_24 * delta4;
for( unsigned int i=0; i < 7; i++ ) {
DSx[i] = Sx1[i] - Sx0[i];
DSy[i] = Sy1[i] - Sy0[i];
}
// calculate Esirkepov coeff. Wx, Wy, Wz when used
double tmp, tmp2, tmp3, tmpY;
//Do not compute useless weights.
// ------------------------------------------------
// Local current created by the particle
// calculate using the charge conservation equation
// ------------------------------------------------
// ---------------------------
// Calculate the total current
// ---------------------------
ipo -= 3; //This minus 3 come from the order 4 scheme, based on a 7 points stencil from -3 to +3.
jpo -= 3;
// i =0
{
iloc = ipo*nprimy+jpo;
tmp2 = 0.5*Sx1[0];
tmp3 = Sx1[0];
Jz[iloc] += crz_p * ( Sy1[0]*tmp3 );
tmp = 0;
tmpY = Sx0[0] + 0.5*DSx[0];
for( unsigned int j=1 ; j<7 ; j++ ) {
tmp -= cry_p * DSy[j-1] * tmpY;
Jy[iloc+j+ipo] += tmp; //Because size of Jy in Y is nprimy+1.
Jz[iloc+j] += crz_p * ( Sy0[j]*tmp2 + Sy1[j]*tmp3 );
}
}//i
for( unsigned int i=1 ; i<7 ; i++ ) {
iloc = ( i+ipo )*nprimy+jpo;
tmpJx[0] -= crx_p * DSx[i-1] * ( 0.5*DSy[0] );
Jx[iloc] += tmpJx[0];
tmp2 = 0.5*Sx1[i] + Sx0[i];
tmp3 = 0.5*Sx0[i] + Sx1[i];
Jz[iloc] += crz_p * ( Sy1[0]*tmp3 );
tmp = 0;
tmpY = Sx0[i] + 0.5*DSx[i];
for( unsigned int j=1 ; j<7 ; j++ ) {
tmpJx[j] -= crx_p * DSx[i-1] * ( Sy0[j] + 0.5*DSy[j] );
Jx[iloc+j] += tmpJx[j];
tmp -= cry_p * DSy[j-1] * tmpY;
Jy[iloc+j+i+ipo] += tmp; //Because size of Jy in Y is nprimy+1.
Jz[iloc+j] += crz_p * ( Sy0[j]*tmp2 + Sy1[j]*tmp3 );
}
}//i
}
unsigned int _take_particles(Model *, const Particles &ps, base::TextOutput) {
for (unsigned int i = 0; i < ps.size(); ++i) {
IMP_CHECK_OBJECT(ps[i]);
}
return ps.size();
}
unsigned int _take_particles(const Particles &ps) {
for (unsigned int i = 0; i < ps.size(); ++i) {
IMP_CHECK_OBJECT(ps[i]);
}
return ps.size();
}
