VecDoub Actions_Spherical::find_limits(double r, double E, double L){
VecDoub limits;
Actions_Spherical_limits_struct Act(Pot,E,L);
double r_in=r, r_out=r;
root_find RF(SMALL,100);
if(p_r(0.,&Act)>0) r_in=0.;
else while(p_r(r_in,&Act)>=0.0) r_in*=0.9;
while(p_r(r_out,&Act)>=0.0) r_out*=1.1;
limits.push_back(RF.findroot(&p_r,r_in,r,&Act));
limits.push_back(RF.findroot(&p_r,r,r_out,&Act));
return limits;
}
int
process(const tendrils& in, const tendrils& out)
{
cv::Mat R, T, K, depth;
in.get<cv::Mat>("R").convertTo(R, CV_64F);
in.get<cv::Mat>("T").convertTo(T, CV_64F);
in.get<cv::Mat>("K").convertTo(K, CV_64F);
depth = in.get<cv::Mat>("depth");
cv::Mat mask;
if (!depth.empty())
{
mask = cv::Mat::zeros(depth.size(), CV_8UC1);
}
else
return ecto::OK;
if (R.empty() || T.empty() || K.empty())
return 0;
double fx, fy, cx, cy;
fx = K.at<double>(0, 0);
fy = K.at<double>(1, 1);
cx = K.at<double>(0, 2);
cy = K.at<double>(1, 2);
box_mask.create(depth.size());
box_mask.setTo(cv::Scalar(0));
std::vector<cv::Point3f> box(8), tbox(8);
box[0] = cv::Point3f(*x_crop, *y_crop, *z_min);
box[1] = cv::Point3f(-*x_crop, *y_crop, *z_min);
box[2] = cv::Point3f(-*x_crop, -*y_crop, *z_min);
box[3] = cv::Point3f(*x_crop, -*y_crop, *z_min);
box[4] = cv::Point3f(*x_crop, *y_crop, *z_crop);
box[5] = cv::Point3f(-*x_crop, *y_crop, *z_crop);
box[6] = cv::Point3f(-*x_crop, -*y_crop, *z_crop);
box[7] = cv::Point3f(*x_crop, -*y_crop, *z_crop);
cv::Mat RT(3, 4, CV_64F);
cv::Mat _T(RT.colRange(3, 4));
cv::Mat _R(RT.colRange(0, 3));
R.copyTo(_R);
T.copyTo(_T);
cv::transform(box, tbox, RT);
std::vector<cv::Point2f> projected, hull;
cv::projectPoints(box, R, T, K, cv::Mat(4, 1, CV_64FC1, cv::Scalar(0)), projected);
cv::convexHull(projected, hull, true);
std::vector<cv::Point> points(hull.size());
std::copy(hull.begin(), hull.end(), points.begin());
cv::fillConvexPoly(box_mask, points.data(), points.size(), cv::Scalar::all(255));
cv::Matx<double, 3, 3> A_x;
int width = mask.size().width;
int height = mask.size().height;
cv::Mat_<float_t>::iterator dit = depth.begin<float_t>();
cv::Mat_<uint8_t>::iterator it = mask.begin<uint8_t>();
cv::Mat_<uint8_t>::iterator mit = box_mask.begin();
cv::Matx<double, 3, 1> p, p_r, Tx(T); //Translation
cv::Matx<double, 3, 3> Rx; //inverse Rotation
Rx = cv::Mat(R.t());
// std::cout << cv::Mat(Rx) << "\n" << cv::Mat(Tx) << std::endl;
// std::cout << fx << " " << fy << " " << cx << " " << cy << " " << std::endl;
double z_min_ = *z_min, z_max_ = *z_crop, x_min_ = -*x_crop, x_max_ = *x_crop, y_min_ = -*y_crop,
y_max_ = *y_crop;
for (int v = 0; v < height; v++)
{
for (int u = 0; u < width; u++, ++it, ++mit, ++dit)
{
if (*mit == 0)
continue;
//calculate the point based on the depth
p(2) = *dit; //depth value in meters
p(0) = (u - cx) * p(2) / fx;
p(1) = (v - cy) * p(2) / fy;
p_r = Rx * (p - Tx);
// std::cout <<"p=" << cv::Mat(p) << ",p_r="<< cv::Mat(p_r) << std::endl;
if (p_r(2) > z_min_ && p_r(2) < z_max_ && p_r(0) > x_min_ && p_r(0) < x_max_ && p_r(1) > y_min_
&& p_r(1) < y_max_)
*it = 255;
}
}
out["mask"] << mask;
return ecto::OK;
}
inline TR sysperm(const T1& rho1, const arma::uvec& sys,
const arma::uvec& dim) {
const auto& p = as_Mat(rho1);
const arma::uword n = dim.n_elem;
bool checkV = true;
if (p.n_cols == 1)
checkV = false;
#ifndef QICLIB_NO_DEBUG
if (p.n_elem == 0)
throw Exception("qic::sysperm", Exception::type::ZERO_SIZE);
if (checkV)
if (p.n_rows != p.n_cols)
throw Exception("qic::sysperm",
Exception::type::MATRIX_NOT_SQUARE_OR_CVECTOR);
if (dim.n_elem == 0 || arma::any(dim == 0))
throw Exception("qic::sysperm", Exception::type::INVALID_DIMS);
if (arma::prod(dim) != p.n_rows)
throw Exception("qic::sysperm", Exception::type::DIMS_MISMATCH_MATRIX);
if (n != sys.n_elem || arma::any(sys == 0) || arma::any(sys > n) ||
sys.n_elem != arma::unique(sys).eval().n_elem)
throw Exception("qic::sysperm", Exception::type::PERM_INVALID);
#endif
arma::uword product[_internal::MAXQDIT];
product[n-1] = 1;
for (arma::sword i = n - 2; i >= 0; --i)
product[i] = product[i + 1] * dim.at(i + 1);
arma::uword productr[_internal::MAXQDIT];
productr[n-1] = 1;
for (arma::sword i = n - 2; i >= 0; --i)
productr[i] = productr[i + 1] * dim.at(sys.at(i + 1) - 1);
if (checkV) {
arma::Mat<trait::eT<T1> > p_r(p.n_rows, p.n_cols, arma::fill::zeros);
const arma::uword loop_no = 2 * n;
constexpr auto loop_no_buffer = 2 * _internal::MAXQDIT + 1;
arma::uword loop_counter[loop_no_buffer] = {0};
arma::uword MAX[loop_no_buffer];
for (arma::uword i = 0; i < n; ++i) {
MAX[i] = dim.at(i);
MAX[i + n] = dim.at(i);
}
MAX[loop_no] = 2;
arma::uword p1 = 0;
while (loop_counter[loop_no] == 0) {
arma::uword I(0), J(0), K(0), L(0);
for (arma::uword i = 0; i < n; ++i) {
I += product[i] * loop_counter[i];
J += product[i] * loop_counter[i + n];
K += productr[i] * loop_counter[sys.at(i) - 1];
L += productr[i] * loop_counter[sys.at(i) + n - 1];
}
p_r.at(K, L) = p.at(I, J);
++loop_counter[0];
while (loop_counter[p1] == MAX[p1]) {
loop_counter[p1] = 0;
loop_counter[++p1]++;
if (loop_counter[p1] != MAX[p1])
p1 = 0;
}
}
return p_r;
} else {
arma::Col<trait::eT<T1> > p_r(p.n_rows, arma::fill::zeros);
const arma::uword loop_no = n;
constexpr auto loop_no_buffer = _internal::MAXQDIT + 1;
arma::uword loop_counter[loop_no_buffer] = {0};
arma::uword MAX[loop_no_buffer];
for (arma::uword i = 0; i < n; ++i) MAX[i] = dim.at(i);
MAX[loop_no] = 2;
for (arma::uword i = 0; i < loop_no + 1; ++i) loop_counter[i] = 0;
arma::uword p1 = 0;
while (loop_counter[loop_no] == 0) {
arma::uword I(0), K(0);
for (arma::uword i = 0; i < n; ++i) {
I += product[i] * loop_counter[i];
K += productr[i] * loop_counter[sys.at(i) - 1];
}
p_r.at(K) = p.at(I);
++loop_counter[0];
while (loop_counter[p1] == MAX[p1]) {
loop_counter[p1] = 0;
loop_counter[++p1]++;
if (loop_counter[p1] != MAX[p1])
p1 = 0;
}
}
return p_r;
}
}
int
process(const tendrils& in, const tendrils& out)
{
cv::Mat depth = in.get<cv::Mat>("depth");
if (depth.empty())
return ecto::OK;
cv::Mat R, T, K;
in.get<cv::Mat>("R").convertTo(R, CV_64F);
in.get<cv::Mat>("T").convertTo(T, CV_64F);
in.get<cv::Mat>("K").convertTo(K, CV_64F);
if (R.empty() || T.empty() || K.empty())
return ecto::OK;
cv::Mat mask = cv::Mat::zeros(depth.size(), CV_8UC1);
box_mask.create(depth.size());
box_mask.setTo(cv::Scalar(0));
std::vector<cv::Point3f> box(8);
box[0] = cv::Point3f(*x_crop, *y_crop, *z_min);
box[1] = cv::Point3f(-*x_crop, *y_crop, *z_min);
box[2] = cv::Point3f(-*x_crop, -*y_crop, *z_min);
box[3] = cv::Point3f(*x_crop, -*y_crop, *z_min);
box[4] = cv::Point3f(*x_crop, *y_crop, *z_crop);
box[5] = cv::Point3f(-*x_crop, *y_crop, *z_crop);
box[6] = cv::Point3f(-*x_crop, -*y_crop, *z_crop);
box[7] = cv::Point3f(*x_crop, -*y_crop, *z_crop);
std::vector<cv::Point2f> projected, hull;
cv::projectPoints(box, R, T, K, cv::Mat(4, 1, CV_64FC1, cv::Scalar(0)), projected);
cv::convexHull(projected, hull, true);
std::vector<cv::Point> points(hull.size());
std::copy(hull.begin(), hull.end(), points.begin());
cv::fillConvexPoly(box_mask, points.data(), points.size(), cv::Scalar::all(255));
int width = mask.size().width;
int height = mask.size().height;
cv::Mat_<uint8_t>::iterator it = mask.begin<uint8_t>();
cv::Mat_<uint8_t>::iterator mit = box_mask.begin();
cv::Mat_<cv::Vec3f> points3d;
depthTo3dMask(K, depth, box_mask, points3d);
if (points3d.empty())
return ecto::OK;
cv::Matx<double, 3, 1> p, p_r, Tx(T); //Translation
cv::Matx<double, 3, 3> Rx; //inverse Rotation
Rx = cv::Mat(R.t());
cv::Mat_<cv::Vec3f>::iterator point = points3d.begin();
// std::cout << cv::Mat(Rx) << "\n" << cv::Mat(Tx) << std::endl;
// std::cout << fx << " " << fy << " " << cx << " " << cy << " " << std::endl;
double z_min_ = *z_min, z_max_ = *z_crop, x_min_ = -*x_crop, x_max_ = *x_crop, y_min_ = -*y_crop,
y_max_ = *y_crop;
for (int v = 0; v < height; v++)
{
for (int u = 0; u < width; u++, ++it, ++mit)
{
if (*mit == 0)
continue;
//calculate the point based on the depth
p(0) = (*point).val[0];
p(1) = (*point).val[1];
p(2) = (*point).val[2];
++point;
p_r = Rx * (p - Tx);
// std::cout <<"p=" << cv::Mat(p) << ",p_r="<< cv::Mat(p_r) << std::endl;
if (p_r(2) > z_min_ && p_r(2) < z_max_ && p_r(0) > x_min_ && p_r(0) < x_max_ && p_r(1) > y_min_
&& p_r(1) < y_max_)
*it = 255;
}
}
out["mask"] << mask;
return ecto::OK;
}
