RcppExport SEXP rthhist(SEXP x_, SEXP nbins_, SEXP nch_, SEXP nthreads)
{
Rcpp::NumericVector x(x_);
const int n = x.size();
const int nbins = INTEGER(nbins_)[0];
const int nch = INTEGER(nch_)[0];
floublevec dx(x.begin(), x.end());
Rcpp::IntegerVector bincounts(nbins);
Rcpp::NumericVector R_left(1);
Rcpp::NumericVector R_binwidth(1);
#if RTH_OMP
omp_set_num_threads(INT(nthreads));
#elif RTH_TBB
tbb::task_scheduler_init init(INT(nthreads));
#endif
// determine binwidth etc.
thrust::pair<floubleveciter, floubleveciter> mm =
thrust::minmax_element(dx.begin(), dx.end());
flouble left = *(mm.first), right = *(mm.second);
flouble binwidth = (right - left) / nbins;
// form matrix of bin counts, one row per chunk
intvec dbincounts(nch*nbins);
// the heart of the computation, a for_each() loop, one iteration per
// chunk
thrust::counting_iterator<int> seqa(0);
thrust::counting_iterator<int> seqb = seqa + nch;
thrust::for_each(seqa,seqb,
do1chunk(dx.begin(),dbincounts.begin(
),n,nbins,nch,left,binwidth));
// copy result to host and combine the subhistograms
int hbincounts[nch*nbins];
thrust::copy(dbincounts.begin(),dbincounts.end(),hbincounts);
int binnum,chunknum;
for (binnum = 0; binnum < nbins; binnum++) {
int sum = 0;
for (chunknum = 0; chunknum < nch; chunknum++)
sum += hbincounts[chunknum*nbins + binnum];
bincounts[binnum] = sum;
}
REAL(R_left)[0] = (double) left;
REAL(R_binwidth)[0] = (double) binwidth;
return Rcpp::List::create(Rcpp::Named("counts") = bincounts,
Rcpp::Named("left") = R_left,
Rcpp::Named("binwidth") = R_binwidth);
}
void vPreProcess::initUndistortion(const yarp::os::Bottle &left,
const yarp::os::Bottle &right,
const yarp::os::Bottle &stereo, bool truncate)
{
this->truncate = truncate;
const yarp::os::Bottle *coeffs[3] = { &left, &right, &stereo};
cv::Mat *maps[2] = {&leftMap, &rightMap};
cv::Mat cameraMatrix[2];
cv::Mat distCoeffs[2];
cv::Mat rectRot[2];
cv::Size s(res.height, res.width);
cv::Mat Proj[2];
//create camera and distortion matrices
for(int i = 0; i < 2; i++) {
double scaley = res.height / (double)(coeffs[i]->find("h").asInt());
double scalex = res.width / (double)(coeffs[i]->find("w").asInt());
cameraMatrix[i] = cv::Mat(3, 3, CV_64FC1);
cameraMatrix[i].setTo(0);
cameraMatrix[i].at<double>(0, 0) = coeffs[i]->find("fx").asDouble()*scalex;
cameraMatrix[i].at<double>(1, 1) = coeffs[i]->find("fy").asDouble()*scaley;
cameraMatrix[i].at<double>(2, 2) = 1.0;
cameraMatrix[i].at<double>(0, 2) = coeffs[i]->find("cx").asDouble()*scalex;
cameraMatrix[i].at<double>(1, 2) = coeffs[i]->find("cy").asDouble()*scaley;
distCoeffs[i] = cv::Mat(4, 1, CV_64FC1);
distCoeffs[i].at<double>(0, 0) = coeffs[i]->find("k1").asDouble();
distCoeffs[i].at<double>(0, 1) = coeffs[i]->find("k2").asDouble();
distCoeffs[i].at<double>(0, 2) = coeffs[i]->find("p1").asDouble();
distCoeffs[i].at<double>(0, 3) = coeffs[i]->find("p2").asDouble();
cv::Mat defCamMat = cv::getDefaultNewCameraMatrix(cameraMatrix[i], s, true);
Proj[i] = defCamMat;
}
if(rectify)
{
//Loading extrinsic stereo parameters
yarp::os::Bottle *HN = coeffs[2]->find("HN").asList();
if(HN == nullptr || HN->size() != 16)
yError() << "Rototranslation matrix HN is absent or without required number of values: 16)";
else
{
std::cout<<"After extracting list from bottle value HN: "<<(HN->toString())<<std::endl;
cv::Mat R(3, 3, CV_64FC1); //Rotation matrix between stereo cameras
cv::Mat T(3, 1, CV_64FC1); //Translation vector of right wrt left camera center
for (int row=0; row<3; row++)
{
for(int col=0; col<3; col++)
{
R.at<double>(row, col) = HN->get(row*4 + col).asDouble();
}
T.at<double>(row) = HN->get(row*4+3).asDouble();
}
std::cout<<"R and T values stored properly; R:"<<R<<"T: "<<T<<std::endl;
cv::Mat R_left(3, 3, CV_64FC1);
cv::Mat R_right(3, 3, CV_64FC1);
cv::Mat P_left(3, 4, CV_64FC1);
cv::Mat P_right(3, 4, CV_64FC1);
cv::Mat Q(4, 4, CV_64FC1);
//Computing homographies for left and right image
cv::stereoRectify(cameraMatrix[0], distCoeffs[0], cameraMatrix[1], distCoeffs[1],
s, R, T, R_left, R_right, P_left, P_right, Q, CV_CALIB_ZERO_DISPARITY);
rectRot[0] = R_left.clone();
rectRot[1] = R_right.clone();
Proj[0] = P_left.clone();
Proj[1] = P_right.clone();
}
}
for(int i=0; i<2; i++) {
cv::Mat allpoints(res.height * res.width, 1, CV_32FC2);
for(unsigned int y = 0; y < res.height; y++) {
for(unsigned int x = 0; x < res.width; x++) {
allpoints.at<cv::Vec2f>(y * res.width + x) = cv::Vec2f(x, y);
}
}
cv::Mat mappoints(res.height * res.width, 1, CV_32FC2);
cv::undistortPoints(allpoints, mappoints, cameraMatrix[i], distCoeffs[i],
rectRot[i], Proj[i]);
*(maps[i]) = cv::Mat(res.height, res.width, CV_32SC2);
for(unsigned int y = 0; y < res.height; y++) {
for(unsigned int x = 0; x < res.width; x++) {
maps[i]->at<cv::Vec2i>(y, x) =
mappoints.at<cv::Vec2f>(y * res.width + x);
}
}
}
}
