///////////////////////
///// Inference /////
///////////////////////
void expAndNormalize ( Eigen::MatrixXf & out, const Eigen::MatrixXf & in ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<out.cols(); i++ ){
Eigen::VectorXf b = in.col(i);
b.array() -= b.maxCoeff();
b = b.array().exp();
out.col(i) = b / b.array().sum();
}
}
float felli(const std::vector<float>& xx) {
Eigen::VectorXf x = Eigen::VectorXf::Zero(xx.size());
for (size_t i = 0; i < xx.size(); ++i)
x[i] = xx[i];
Eigen::VectorXf v = Eigen::VectorXf::Zero(x.size());
for (size_t i = 0; i < v.size(); ++i)
v[i] = powf(1e6, i / (x.size() - 1.0f));
return v.dot((x.array() * x.array()).matrix());
}
float math_util::get_weighted_mean(std::vector<float> population_, std::vector<float> weights_)
{
//Generate weighted mean using the weights and population values provided
//Allocate population and weights Eigen vector
Eigen::VectorXf population = transform_to_eigen(population_);
Eigen::VectorXf weights = transform_to_eigen(weights_);
//Coefficient-wise multiplication of each value with its weight
Eigen::VectorXf weighted_population = population.array() * weights.array();
//Return the weighted average
return weighted_population.sum()/weights.sum();
}
std::tuple<Eigen::MatrixXf, Eigen::VectorXf> dart::SimpleWeightedError::computeGNParam(const Eigen::VectorXf &diff) {
// compute error from joint deviation
const double e = (_weight*diff).norm();
// Jacobian of error, e.g. the partial derivation of the error w.r.t. to each joint value
// For an error of zero, its partial derivative is not defined. Therefore we set its derivative to 0.
const Eigen::MatrixXf J = (diff.array()==0).select(0, -diff.array()/e);
const Eigen::VectorXf JTe = - diff.transpose();
return std::make_tuple(J, JTe);
}
std::tuple<Eigen::MatrixXf, Eigen::VectorXf> dart::L2NormOfWeightedError::computeGNParam(const Eigen::VectorXf &diff) {
// compute error from joint deviation
// error: L2 norm of weighted joint angle difference
const float e = (_weight * diff).norm();
// Jacobian of error, e.g. the partial derivation of the error w.r.t. to each joint value
// For an error of zero, its partial derivative is not defined. Therefore we set its derivative to 0.
const Eigen::MatrixXf J = (diff.array()==0).select(0, - pow(_weight, 2) * diff.array()/diff.norm());
const Eigen::VectorXf JTe = J.array()*e;
return std::make_tuple(J, JTe);
}
float math_util::get_variance(std::vector<float> distribution_)
{
// Initialize an Eigen vector representing the distribution
// In the following the formula for variance is implemented:
// Var(x) = 1/N * SUM((x-mean(x))^2)
Eigen::VectorXf distribution;
distribution = Eigen::Map<Eigen::VectorXf>(&distribution_[0],distribution_.size(),1);
// Retrieve mean value, mean(x)
float distribution_mean = distribution.mean();
// Get deviation vector. (x-mean(x))^2
distribution = (distribution.array() - distribution_mean) * (distribution.array() - distribution_mean);
// Get variance from deviation vector. Var(x)
return distribution.mean();
}
void sumAndNormalize( Eigen::MatrixXf & out, const Eigen::MatrixXf & in, const Eigen::MatrixXf & Q ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<in.cols(); i++ ){
Eigen::VectorXf b = in.col(i);
Eigen::VectorXf q = Q.col(i);
out.col(i) = b.array().sum()*q - b;
}
}
Eigen::MatrixXf featureGradient( const Eigen::MatrixXf & a, const Eigen::MatrixXf & b ) const {
if (ntype_ == NO_NORMALIZATION )
return kernelGradient( a, b );
else if (ntype_ == NORMALIZE_SYMMETRIC ) {
Eigen::MatrixXf fa = lattice_.compute( a*norm_.asDiagonal(), true );
Eigen::MatrixXf fb = lattice_.compute( b*norm_.asDiagonal() );
Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
Eigen::VectorXf norm3 = norm_.array()*norm_.array()*norm_.array();
Eigen::MatrixXf r = kernelGradient( 0.5*( a.array()*fb.array() + fa.array()*b.array() ).matrix()*norm3.asDiagonal(), ones );
return - r + kernelGradient( a*norm_.asDiagonal(), b*norm_.asDiagonal() );
}
else if (ntype_ == NORMALIZE_AFTER ) {
Eigen::MatrixXf fb = lattice_.compute( b );
Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
Eigen::VectorXf norm2 = norm_.array()*norm_.array();
Eigen::MatrixXf r = kernelGradient( ( a.array()*fb.array() ).matrix()*norm2.asDiagonal(), ones );
return - r + kernelGradient( a*norm_.asDiagonal(), b );
}
else /*if (ntype_ == NORMALIZE_BEFORE )*/ {
Eigen::MatrixXf fa = lattice_.compute( a, true );
Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
Eigen::VectorXf norm2 = norm_.array()*norm_.array();
Eigen::MatrixXf r = kernelGradient( ( fa.array()*b.array() ).matrix()*norm2.asDiagonal(), ones );
return -r+kernelGradient( a, b*norm_.asDiagonal() );
}
}
float mmf::OptSO3MMFvMF::computeAssignment(uint32_t& N)
{
N = this->cld_.counts().sum();
// Compute log of the cluster weights and push them to GPU
Eigen::VectorXf pi = Eigen::VectorXf::Ones(K()*6)*1000;
std::cout << this->t_ << std::endl;
std::cout<<"counts: "<<this->cld_.counts().transpose()<<std::endl;
if (this->t_ == 0) {
pi.fill(1.);
} else if (this->t_ >24) {
std::cout << "truncating noisy MFs: " << std::endl;
for (uint32_t k=0; k<K()*6; ++k) {
float count = this->cld_.counts().middleRows((k/6)*6,6).sum();
pi(k) = count > 0.10*N ? count : 1.e-20;
std::cout << count << " < " << 0.1*N << std::endl;
}
} else {
// estimate the axis and MF proportions
// pi += this->cld_.counts();
// estimate only the MF proportions
for (uint32_t k=0; k<K()*6; ++k)
pi(k) += this->cld_.counts().middleRows((k/6)*6,6).sum();
if (estimateTau_) {
for (uint32_t k=0; k<K()*6; ++k)
if (this->cld_.counts()(k) == 0) {
taus_(k) = 0.; // uniform
} else {
Eigen::Vector3f mu = Eigen::Vector3f::Zero();
mu((k%6)/2) = (k%6)%2==0?-1.:1.;
mu = Rs_[k/6]*mu;
taus_(k) = jsc::vMF<3>::MLEstimateTau(
this->cld_.xSums().col(k).cast<double>(),
mu.cast<double>(), this->cld_.counts()(k));
}
} else {
taus_.fill(60.);
}
}
std::cout<<"pi: "<<pi.transpose()<<std::endl;
pi = (pi.array() / pi.sum()).array().log();
std::cout<<"pi: "<<pi.transpose()<<std::endl;
if (estimateTau_) {
std::cout << pi.transpose() << std::endl;
std::cout << taus_.transpose() << std::endl;
for (uint32_t k=0; k<K()*6; ++k) {
pi(k) -= jsc::vMF<3>::log2SinhOverZ(taus_(k)) - log(2.*M_PI);
}
}
pi_.set(pi);
Rot2Device();
Eigen::VectorXf residuals = Eigen::VectorXf::Zero(K()*6);
MMFvMFCostFctAssignmentGPU((float*)residuals.data(), d_cost, &N,
d_N_, cld_.d_x(), d_weights_, cld_.d_z(), d_mu_, pi_.data(),
cld_.N(), K());
return residuals.sum();
};
bool ThermalCalibration::BarometerCalibration(Eigen::VectorXf pressure, Eigen::VectorXf temperature, float *result, float *inputSigma, float *calibratedSigma)
{
// assume the nearest reading to 20°C as the "zero bias" point
int index20deg = searchReferenceValue(20.0f, temperature);
qDebug() << "Ref zero is " << index20deg << " T: " << temperature[index20deg] << " P:" << pressure[index20deg];
float refZero = pressure[index20deg];
pressure.array() -= refZero;
qDebug() << "Rebiased zero is " << pressure[index20deg];
Eigen::VectorXf solution(BARO_PRESSURE_POLY_DEGREE + 1);
if (!CalibrationUtils::PolynomialCalibration(&temperature, &pressure, BARO_PRESSURE_POLY_DEGREE, solution, BARO_PRESSURE_MAX_REL_ERROR)) {
return false;
}
copyToArray(result, solution, BARO_PRESSURE_POLY_DEGREE + 1);
ComputeStats(&temperature, &pressure, &solution, inputSigma, calibratedSigma);
return (*calibratedSigma) < (*inputSigma);
}
std::tuple<Eigen::MatrixXf, Eigen::VectorXf> dart::QWeightedError::computeGNParam(const Eigen::VectorXf &diff) {
// compute error from joint deviation
const float e = diff.transpose()*_Q*diff;
Eigen::MatrixXf deriv = Eigen::MatrixXf::Zero(diff.size(), 1);
for(unsigned int i=0; i<diff.size(); i++) {
// original derivation
//deriv(i) = diff.dot(_Q.row(i)) + diff.dot(_Q.col(i)) - (diff[i]*_Q(i,i));
// negative direction, this works
//deriv(i) = - diff.dot(_Q.row(i)) + diff.dot(_Q.col(i)) - (diff[i]*_Q(i,i));
deriv(i) = - ( diff.dot(_Q.row(i) + _Q.col(i).transpose()) - (diff[i]*_Q(i,i)) );
}
// Jacobian of error, e.g. the partial derivation of the error w.r.t. to each joint value
// For an error of zero, its partial derivative is not defined. Therefore we set its derivative to 0.
const Eigen::MatrixXf J = (diff.array()==0).select(0, deriv);
const Eigen::VectorXf JTe = J.array()*e;
return std::make_tuple(J, JTe);
}
void dart::ReportedJointsPrior::computeContribution(Eigen::SparseMatrix<float> & fullJTJ,
Eigen::VectorXf & fullJTe,
const int * modelOffsets,
const int priorParamOffset,
const std::vector<MirroredModel *> & models,
const std::vector<Pose> & poses,
const OptimizationOptions & opts)
{
// get mapping of reported joint names and values
std::map<std::string, float> rep_map;
for(unsigned int i=0; i<_reported.getReducedArticulatedDimensions(); i++) {
// apply lower and upper joint limits
rep_map[_reported.getReducedName(i)] =
std::min(std::max(_reported.getReducedArticulation()[i], _reported.getReducedMin(i)), _reported.getReducedMax(i));
}
#ifdef LCM_DEBUG_GRADIENT
std::vector<std::string> names;
#if FILTER_FIXED_JOINTS
const bool pub_grad = (_skipped==GRADIENT_SKIP);
#endif
#endif
// compute difference of reported to estimated joint value
Eigen::VectorXf diff = Eigen::VectorXf::Zero(_estimated.getReducedArticulatedDimensions());
for(unsigned int i=0; i<_estimated.getReducedArticulatedDimensions(); i++) {
const std::string jname = _estimated.getReducedName(i);
#ifdef LCM_DEBUG_GRADIENT
#if FILTER_FIXED_JOINTS
if(pub_grad)
if( !(_estimated.getReducedMin(i)==0 && _estimated.getReducedMin(i)==0) )
#endif
names.push_back(jname);
#endif
float rep = rep_map.at(jname);
float est = _estimated.getReducedArticulation()[i];
diff[i] = rep_map.at(jname) - _estimated.getReducedArticulation()[i];
}
// set nan values to 0, e.g. comparison of nan values always yields false
diff = (diff.array()!=diff.array()).select(0,diff);
// get Gauss-Newton parameter for specific objective function
Eigen::MatrixXf J = Eigen::MatrixXf::Zero(_estimated.getReducedArticulatedDimensions(), 1);
Eigen::VectorXf JTe = Eigen::VectorXf::Zero(_estimated.getReducedArticulatedDimensions());
std::tie(J,JTe) = computeGNParam(diff);
const Eigen::MatrixXf JTJ = J.transpose()*J;
#ifdef LCM_DEBUG_GRADIENT
#if FILTER_FIXED_JOINTS
if(pub_grad) {
#endif
// publish gradient (JTe)
bot_core::joint_angles_t grad;
grad.num_joints = names.size();
grad.joint_name = names;
for(unsigned int i = 0; i<JTe.size(); i++) {
#if FILTER_FIXED_JOINTS
if(!(_estimated.getReducedMin(i)==0 && _estimated.getReducedMin(i)==0))
#endif
grad.joint_position.push_back(JTe[i]);
}
LCM_CommonBase::publish("DART_GRADIENT", &grad);
#if FILTER_FIXED_JOINTS
_skipped=0;
}
else {
_skipped++;
}
#endif
#endif // LCM_DEBUG_GRADIENT
for(unsigned int r=0; r<JTJ.rows(); r++)
for(unsigned int c=0; c<JTJ.cols(); c++)
if(JTJ(r,c)!=0)
fullJTJ.coeffRef(modelOffsets[_modelID]+6+r, modelOffsets[_modelID]+6+c) += JTJ(r,c);
for(unsigned int r=0; r<JTe.rows(); r++)
if(JTe[r]!=0)
fullJTe[modelOffsets[_modelID]+6+r] += JTe[r];
}
void TraceStoreCol::WellProj(TraceStoreCol &store,
std::vector<KeySeq> & key_vectors,
vector<char> &filter,
vector<float> &mad) {
int useable_flows = 0;
for (size_t i = 0; i < key_vectors.size(); i++) {
useable_flows = std::max((int)key_vectors[i].usableKeyFlows, useable_flows);
// useable_flows = std::max(store.GetNumFlows(), (size_t)useable_flows);
}
Eigen::VectorXf norm(store.mFrameStride * useable_flows);
Eigen::VectorXf sum(store.mFrameStride * useable_flows);
norm.setZero();
sum.setZero();
int start_frame = store.GetNumFrames() - 2;
int end_frame = store.GetNumFrames();
int count = 0;
for (int frame_ix = start_frame; frame_ix < end_frame; frame_ix++) {
count++;
int16_t *__restrict data_start = store.GetMemPtr() + frame_ix * store.mFlowFrameStride;
int16_t *__restrict data_end = data_start + store.mFrameStride * useable_flows;
float *__restrict norm_start = &norm[0];
while(data_start != data_end) {
*norm_start++ += *data_start++;
}
}
// std::vector<ChipReduction> smoothed_avg(useable_flows);
// int x_clip = mCols;
// int y_clip = mRows;
// if (mUseMeshNeighbors == 0) {
// x_clip = THUMBNAIL_SIZE;
// y_clip = THUMBNAIL_SIZE;
// }
// int last_frame = store.GetNumFrames() - 1;
// for (int flow_ix = 0; flow_ix < useable_flows; flow_ix++) {
// smoothed_avg[flow_ix].Init(mRows, mCols, 1,
// SMOOTH_REDUCE_STEP, SMOOTH_REDUCE_STEP,
// y_clip, x_clip,
// SMOOTH_REDUCE_STEP * SMOOTH_REDUCE_STEP * .4);
// smoothed_avg[flow_ix].ReduceFrame(&mData[0] + flow_ix * mFrameStride + last_frame * mFlowFrameStride, &filter[0], 0);
// smoothed_avg[flow_ix].SmoothBlocks(SMOOTH_REDUCE_REGION, SMOOTH_REDUCE_REGION);
// }
// int x_count = 0;
// for (int flow_ix = 0; flow_ix < useable_flows; flow_ix++) {
// for (size_t row_ix = 0; row_ix < mRows; row_ix++) {
// for (size_t col_ix = 0; col_ix < mCols; col_ix++) {
// float avg = smoothed_avg[flow_ix].GetSmoothEst(row_ix, col_ix, 0);
// norm[x_count] = avg;
// x_count++;
// }
// }
// }
norm = norm / count;
// start_frame = INTEGRATION_START;
// end_frame = INTEGRATION_END;
start_frame = 5;
end_frame = store.GetNumFrames() - 6;
for (int frame_ix = start_frame; frame_ix < end_frame; frame_ix++) {
int16_t *__restrict data_start = store.GetMemPtr() + frame_ix * store.mFlowFrameStride;
int16_t *__restrict data_end = data_start + store.mFrameStride * useable_flows;
float *__restrict norm_start = &norm[0];
float *__restrict sum_start = &sum[0];
int well_offset = 0;
while(data_start != data_end) {
if (*norm_start == 0.0f) {
*norm_start = 1.0f;
// avoid divide by zero but mark that something is wrong with this well..
if (filter[well_offset] == 0) {
filter[well_offset] = 6;
}
}
*sum_start += *data_start / *norm_start;
well_offset++;
// reset each flow
if (well_offset == (int) store.mFrameStride) {
well_offset = 0;
}
sum_start++;
data_start++;
norm_start++;
}
}
Eigen::MatrixXf flow_mat(store.mFrameStride, useable_flows);
for (int flow_ix = 0; flow_ix < flow_mat.cols(); flow_ix++) {
for (size_t well_ix = 0; well_ix < store.mFrameStride; well_ix++) {
flow_mat(well_ix, flow_ix) = sum(flow_ix * store.mFrameStride + well_ix);
}
}
Eigen::VectorXf n2;
n2 = flow_mat.rowwise().squaredNorm();
n2 = n2.array().sqrt();
float *n2_start = n2.data();
float *n2_end = n2_start + n2.rows();
while (n2_start != n2_end) {
if (*n2_start == 0.0f) { *n2_start = 1.0f; }
n2_start++;
}
for (int flow_ix = 0; flow_ix < flow_mat.cols(); flow_ix++) {
flow_mat.col(flow_ix).array() = flow_mat.col(flow_ix).array() / n2.array();
}
Eigen::VectorXf proj;
Eigen::VectorXf max_abs_proj(flow_mat.rows());
max_abs_proj.setZero();
for (size_t key_ix = 0; key_ix < key_vectors.size(); key_ix++) {
Eigen::VectorXf key(useable_flows);
key.setZero();
for (int f_ix = 0; f_ix < useable_flows; f_ix++) {
key[f_ix] = key_vectors[key_ix].flows[f_ix];
}
proj = flow_mat * key;
proj = proj.array().abs();
for (int i = 0; i < proj.rows(); i++) {
max_abs_proj(i) = max(max_abs_proj(i), proj(i));
}
}
for (int i = 0; i < max_abs_proj.rows(); i++) {
mad[i] = max_abs_proj(i);
}
}
int main(int argc, char** argv) {
if (argc < 5) {
std::cerr << "From the 3 images representing fiber orientation estimate angles and calculate angle homogeneity in the neighborhoods." << std::endl;
std::cerr << "Usage: CalculateEntropyFiberAngles coordx_image coordy_image coordz_image out_image neigborhood_radius" << std::endl;
return EXIT_FAILURE;
}
//parsing parameters
int c = 1;
const char* imagex_filename = argv[c++];
const char* imagey_filename = argv[c++];
const char* imagez_filename = argv[c++];
const char* output_filename = argv[c++];
int neighborhood_radius = atoi(argv[c++]);
const unsigned int Dimension = 3;
std::cout << "Input x: " << imagex_filename << std::endl;
std::cout << "Input y: " << imagey_filename << std::endl;
std::cout << "Input z: " << imagez_filename << std::endl;
std::cout << "Output: " << output_filename << std::endl;
std::cout << "Neighborhood radius: " << neighborhood_radius << std::endl;
//main program
typedef float InputPixelType;
typedef float OutputPixelType;
typedef itk::Image< InputPixelType, Dimension > InputImageType;
typedef itk::Image< OutputPixelType, Dimension > OutputImageType;
typedef itk::ImageFileReader< InputImageType > ReaderType;
typedef itk::ImageFileWriter< OutputImageType > WriterType;
ReaderType::Pointer readerx = ReaderType::New();
ReaderType::Pointer readery = ReaderType::New();
ReaderType::Pointer readerz = ReaderType::New();
WriterType::Pointer writer = WriterType::New();
readerx->SetFileName(imagex_filename);
readery->SetFileName(imagex_filename);
readerz->SetFileName(imagex_filename);
writer->SetFileName(output_filename);
std::cout<<"Reading X"<<std::endl;
readerx->Update();
std::cout<<"Reading Y"<<std::endl;
readery->Update();
std::cout<<"Reading Z"<<std::endl;
readerz->Update();
const InputImageType * inputx = readerx->GetOutput();
const InputImageType * inputy = readery->GetOutput();
const InputImageType * inputz = readerz->GetOutput();
//create the output image
OutputImageType::Pointer image = OutputImageType::New();
image->SetRegions(inputx->GetLargestPossibleRegion());
image->SetSpacing(inputx->GetSpacing());
image->SetOrigin(inputx->GetOrigin());
image->Allocate();
image->FillBuffer(0);
InputImageType::SizeType radius;
radius[0] = neighborhood_radius;
radius[1] = neighborhood_radius;
radius[2] = neighborhood_radius;
itk::ConstNeighborhoodIterator<InputImageType>
iteratorx(radius, inputx, image->GetRequestedRegion());
itk::ConstNeighborhoodIterator<InputImageType>
iteratory(radius, inputy, image->GetRequestedRegion());
itk::ConstNeighborhoodIterator<InputImageType>
iteratorz(radius, inputz, image->GetRequestedRegion());
itk::ImageRegionIterator<OutputImageType> it( image, image->GetRequestedRegion() );
iteratorx.GoToBegin();
iteratory.GoToBegin();
iteratorz.GoToBegin();
it.GoToBegin();
const unsigned int nneighbors = (neighborhood_radius*2+1)*(neighborhood_radius*2+1)*(neighborhood_radius*2+1);
Eigen::MatrixXf neighbors( nneighbors-1, 3 ); //to store the vectors
Eigen::Vector3f central_vector;
int c_id = (int) (nneighbors / 2); // get offset of center pixel
//check image size for the progress indicator
OutputImageType::RegionType region = image->GetLargestPossibleRegion();
OutputImageType::SizeType size = region.GetSize();
const unsigned long int npixels = size[0]*size[1]*size[2];
unsigned long int k = 0;
while ( ! iteratorx.IsAtEnd() )
{
if(k%10000 == 0)
{
std::cout<<"Processed "<<k<<"/"<<npixels<<"\r"<<std::flush;
}
k++;
unsigned int j = 0;
for (unsigned int i = 0; i < nneighbors; ++i)
{
if(i!=c_id) //exclude the center
{
neighbors(j, 0) = iteratorx.GetPixel(i);
neighbors(j, 1) = iteratory.GetPixel(i);
neighbors(j, 2) = iteratorz.GetPixel(i);
j++;
}
else
{
central_vector(0) = iteratorx.GetPixel(i);
central_vector(1) = iteratory.GetPixel(i);
central_vector(2) = iteratorz.GetPixel(i);
}
}
// calculate the angles
Eigen::VectorXf dp = neighbors*central_vector;
const float variance = ((dp.array()-dp.mean()).pow(2)).mean();
it.Set(variance);
++iteratorx;
++iteratory;
++iteratorz;
++it;
}
std::cout<<std::endl;
// The output of the resampling filter is connected to a writer and the
// execution of the pipeline is triggered by a writer update.
try {
writer->SetInput(image);
writer->Update();
} catch (itk::ExceptionObject & excep) {
std::cerr << "Exception caught !" << std::endl;
std::cerr << excep << std::endl;
}
return EXIT_SUCCESS;
}