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; }