/***********************************************************************//**
* @brief Publish exposure cube
*
* @param[in] name Exposure cube name.
***************************************************************************/
void ctexpcube::publish(const std::string& name)
{
// Write header into logger
log_header1(TERSE, "Publish exposure cube");
// Set default name if user name is empty
std::string user_name(name);
if (user_name.empty()) {
user_name = CTEXPCUBE_NAME;
}
// Write exposure cube name into logger
log_value(NORMAL, "Counts cube name", user_name);
// Publish exposure cube
m_expcube.cube().publish(user_name);
// Return
return;
}
/***********************************************************************//**
* @brief Save exposure cube
*
* Saves the exposure cube into a FITS file.
***************************************************************************/
void ctexpcube::save(void)
{
// Write header into logger
log_header1(TERSE, "Save exposure cube");
// Get exposure cube filename
m_outcube = (*this)["outcube"].filename();
// Save exposure cube if filename and the exposure cube are not empty
if (!m_outcube.is_empty() && !m_expcube.cube().is_empty()) {
m_expcube.save(m_outcube, clobber());
}
// Write into logger what has been done
std::string fname = (m_outcube.is_empty()) ? "NONE" : m_outcube.url();
if (m_expcube.cube().is_empty()) {
fname.append(" (cube is empty, no file created)");
}
log_value(NORMAL, "Exposure cube file", fname);
// Return
return;
}
std::complex<long double> HomotopyClassPlanner::calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles, double prescaler)
{
if (obstacles->empty())
return std::complex<double>(0,0);
ROS_ASSERT_MSG(prescaler>0.1 && prescaler<=1, "Only a prescaler on the interval (0.1,1] ist allowed.");
// guess values for f0
// paper proposes a+b=N-1 && |a-b|<=1, 1...N obstacles
int m = (int)obstacles->size()-1;
if (m>5)
m = 5; // hardcoded, but this was working in my test cases... TODO further tests requried
// m = round(double(m) * prescaler);
int a = (int) std::ceil(double(m)/2.0);
int b = m-a;
std::advance(path_end, -1); // reduce path_end by 1 (since we check line segments between those path points
typedef std::complex<long double> cplx;
// guess map size (only a really really coarse guess is required
// use distance from start to goal as distance to each direction
// TODO: one could move the map determination outside this function, since it remains constant for the whole planning interval
cplx start = fun_cplx_point(*path_start);
cplx end = fun_cplx_point(*path_end); // path_end points to the last point now after calling std::advance before
double dist = std::sqrt( std::norm(end - start) );
if (dist < 3.0)
dist = 3.0; // set minimum bound on distance (we do not want to have numerical instabilities) and 3.0 performs fine...
cplx map_bottom_left(start.real(), start.imag()-dist);
cplx map_top_right(start.real()+dist, start.imag()+dist);
cplx H = 0;
std::vector<double> imag_proposals(5);
// iterate path
while(path_start != path_end)
{
cplx z1 = fun_cplx_point(*path_start);
cplx z2 = fun_cplx_point(*boost::next(path_start));
for (std::size_t l=0; l<obstacles->size(); ++l) // iterate all obstacles
{
cplx obst_l = obstacles->at(l)->getCentroidCplx();
cplx f0 = (long double) prescaler * std::pow(obst_l-map_bottom_left,a) * std::pow(obst_l-map_top_right,b);
// denum contains product with all obstacles exepct j==l
cplx Al = f0;
for (std::size_t j=0; j<obstacles->size(); ++j)
{
if (j==l)
continue;
cplx obst_j = obstacles->at(j)->getCentroidCplx();
cplx diff = obst_l - obst_j;
if (diff.real()!=0 || diff.imag()!=0)
Al /= diff;
else
continue;
}
// compute log value
double diff2 = std::abs(z2-obst_l);
double diff1 = std::abs(z1-obst_l);
if (diff2 == 0 || diff1 == 0)
continue;
double log_real = std::log(diff2)-std::log(diff1);
// complex ln has more than one solution -> choose minimum abs angle -> paper
imag_proposals.at(0) = std::arg(z2-obst_l)-std::arg(z1-obst_l);
imag_proposals.at(1) = std::arg(z2-obst_l)-std::arg(z1-obst_l)+2*M_PI;
imag_proposals.at(2) = std::arg(z2-obst_l)-std::arg(z1-obst_l)-2*M_PI;
imag_proposals.at(3) = std::arg(z2-obst_l)-std::arg(z1-obst_l)+4*M_PI;
imag_proposals.at(4) = std::arg(z2-obst_l)-std::arg(z1-obst_l)-4*M_PI;
double log_imag = *std::min_element(imag_proposals.begin(),imag_proposals.end(),smaller_than_abs);
cplx log_value(log_real,log_imag);
//cplx log_value = std::log(z2-obst_l)-std::log(z1-obst_l); // the principal solution doesn't seem to work
H += Al*log_value;
}
++path_start;
}
return H;
}
