template <typename PointSource, typename PointTarget, typename Scalar> void pcl::registration::TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation ( const Eigen::MatrixXf &cloud_src_demean, const Eigen::Vector4f ¢roid_src, const Eigen::MatrixXf &cloud_tgt_demean, const Eigen::Vector4f ¢roid_tgt, Matrix4 &transformation_matrix) const { transformation_matrix.setIdentity (); // Assemble the correlation matrix H = source * target' Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean.cast<Scalar> () * cloud_tgt_demean.cast<Scalar> ().transpose ()).topLeftCorner (3, 3); // Compute the Singular Value Decomposition Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU (); Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV (); // Compute R = V * U' if (u.determinant () * v.determinant () < 0) { for (int x = 0; x < 3; ++x) v (x, 2) *= -1; } Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose (); // rotated cloud Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> src_ = R * cloud_src_demean.cast<Scalar> (); float scale1, scale2; double sum_ss = 0.0f, sum_tt = 0.0f, sum_tt_ = 0.0f; for (unsigned corrIdx = 0; corrIdx < cloud_src_demean.cols (); ++corrIdx) { sum_ss += cloud_src_demean (0, corrIdx) * cloud_src_demean (0, corrIdx); sum_ss += cloud_src_demean (1, corrIdx) * cloud_src_demean (1, corrIdx); sum_ss += cloud_src_demean (2, corrIdx) * cloud_src_demean (2, corrIdx); sum_tt += cloud_tgt_demean (0, corrIdx) * cloud_tgt_demean (0, corrIdx); sum_tt += cloud_tgt_demean (1, corrIdx) * cloud_tgt_demean (1, corrIdx); sum_tt += cloud_tgt_demean (2, corrIdx) * cloud_tgt_demean (2, corrIdx); sum_tt_ += cloud_tgt_demean (0, corrIdx) * src_ (0, corrIdx); sum_tt_ += cloud_tgt_demean (1, corrIdx) * src_ (1, corrIdx); sum_tt_ += cloud_tgt_demean (2, corrIdx) * src_ (2, corrIdx); } scale1 = sqrt (sum_tt / sum_ss); scale2 = sum_tt_ / sum_ss; float scale = scale2; transformation_matrix.topLeftCorner (3, 3) = scale * R; const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.cast<Scalar> ().head (3)); transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.cast<Scalar> (). head (3) - Rc; }
void OCCA_RFUNC occaAsyncCopyMemToPtr(void *dest, occaMemory src, const uintptr_t bytes, const uintptr_t offset) { occa::memory src_((occa::memory_v*) src->value().data.void_); asyncMemcpy(dest, src_, bytes, offset); }
pixel_t point_sampler_t::operator()( const Imath::V2d& p) const { Imath::V2i pi( IECore::fastFloat2Int( p.x + ( p.x < 0.0f ? -0.5f : 0.5f)), IECore::fastFloat2Int( p.y + ( p.y < 0.0f ? -0.5f : 0.5f))); if( src_area_.intersects( pi)) return src_( pi.x - src_area_.min.x, pi.y - src_area_.min.y); else return pixel_t( 0, 0, 0, 0); }
void OCCA_RFUNC occaAsyncCopyMemToMem(occaMemory dest, occaMemory src, const uintptr_t bytes, const uintptr_t destOffset, const uintptr_t srcOffset) { occa::memory src_((occa::memory_v*) src->value().data.void_); occa::memory dest_((occa::memory_v*) dest->value().data.void_); asyncMemcpy(dest_, src_, bytes, destOffset, srcOffset); }
void GaussianDerivation::processderivation(int width, int height, Mat& src, Mat& dst, deviation_type deviationType) { Mat_<Vec3b> src_ = src; Mat_<Vec3b> dst_ = dst; switch (deviationType) { case 0: xDerivation(); break; case 1: yDerivation(); break; case 2: xxDerivation(); break; case 3: yyDerivation(); break; case 4: xyDerivation(); break; default: printf("error\n"); return ; } float** deviationArr[5] = {m_xDerviation,m_yDerviation,m_xxDerviation,m_yyDerviation,m_xxDerviation}; for(int row=0; row<height; row++) { for(int col=0; col<width; col++) { double xred = 0; double xgreen = 0; double xblue = 0; for(int subrow = -win_size; subrow <= win_size; subrow++) { for(int subcol = -win_size; subcol <= win_size; subcol++) { int newRow = row + subrow; int newCol = col + subcol; //if(newRow < 0 || newRow >= height) //{ // newRow = row; //} //if(newCol < 0 || newCol >= width) //{ // newCol = col; //} if (newRow<0) { newRow *= -1; } if (newRow>=height) { newRow = (height -1) - (newRow - (height - 1)); } if (newCol<0) { newCol *= -1; } if (newCol>=width) { newCol = (width -1) - (newCol - (width - 1)); } int tr = src_(newRow,newCol)[2]; int tg = src_(newRow,newCol)[1]; int tb = src_(newRow,newCol)[0]; xred += (deviationArr[deviationType][subrow + win_size][subcol + win_size] * tr); xgreen += (deviationArr[deviationType][subrow + win_size][subcol + win_size] * tg); xblue += (deviationArr[deviationType][subrow + win_size][subcol + win_size] * tb); } } int temp1 = xred * 10 + 50; int temp2 = xgreen * 10 + 50; int temp3 = xblue * 10 + 50; dst_(row,col)[2] = temp1 < 0 ? 0 : (temp1 > 255 ? 255 : temp1); dst_(row,col)[1] = temp2 < 0 ? 0 : (temp2 > 255 ? 255 : temp2); dst_(row,col)[0] = temp3 < 0 ? 0 : (temp3 > 255 ? 255 : temp3); } } }
void CLAHE_CalcLut_Body<T,histSize,shift>::operator ()(const cv::Range& range) const { T* tileLut = lut_.ptr<T>(range.start); const size_t lut_step = lut_.step / sizeof(T); for (int k = range.start; k < range.end; ++k, tileLut += lut_step) { const int ty = k / tilesX_; const int tx = k % tilesX_; // retrieve tile submatrix cv::Rect tileROI; tileROI.x = tx * tileSize_.width; tileROI.y = ty * tileSize_.height; tileROI.width = tileSize_.width; tileROI.height = tileSize_.height; const cv::Mat tile = src_(tileROI); // calc histogram int tileHist[histSize] = {0, }; int height = tileROI.height; const size_t sstep = src_.step / sizeof(T); for (const T* ptr = tile.ptr<T>(0); height--; ptr += sstep) { int x = 0; for (; x <= tileROI.width - 4; x += 4) { int t0 = ptr[x], t1 = ptr[x+1]; tileHist[t0 >> shift]++; tileHist[t1 >> shift]++; t0 = ptr[x+2]; t1 = ptr[x+3]; tileHist[t0 >> shift]++; tileHist[t1 >> shift]++; } for (; x < tileROI.width; ++x) tileHist[ptr[x] >> shift]++; } // clip histogram if (clipLimit_ > 0) { // how many pixels were clipped int clipped = 0; for (int i = 0; i < histSize; ++i) { if (tileHist[i] > clipLimit_) { clipped += tileHist[i] - clipLimit_; tileHist[i] = clipLimit_; } } // redistribute clipped pixels int redistBatch = clipped / histSize; int residual = clipped - redistBatch * histSize; for (int i = 0; i < histSize; ++i) tileHist[i] += redistBatch; if (residual != 0) { int residualStep = MAX(histSize / residual, 1); for (int i = 0; i < histSize && residual > 0; i += residualStep, residual--) tileHist[i]++; } } // calc Lut int sum = 0; for (int i = 0; i < histSize; ++i) { sum += tileHist[i]; tileLut[i] = cv::saturate_cast<T>(sum * lutScale_); } } }
std::vector<cv::Mat>* laneTracker::roadColorDetect() { //Rect(x, y ,width, height) //Rect roi(0, src_.rows/2, src_.cols, src_.rows/2); //TODO dynamicly adjust road_rect cv::Mat roadRegion = src_(ROAD_RECT(src_.cols, src_.rows)); cv::Mat ycrcb; cvtColor(roadRegion, ycrcb, CV_BGR2YCrCb); /// Separate the image in 3 places ( B, G and R ) std::vector<cv::Mat> bgr_planes; split(roadRegion, bgr_planes); std::vector<cv::Mat> ycbcr_planes; split(ycrcb, ycbcr_planes); /// Establish the number of bins, x-axis int histSize = 256; /// Set the ranges ( for B,G,R) ) float range[] = { 0, 256 } ; const float* histRange = { range }; bool uniform = true; bool accumulate = false; cv::Mat b_hist, g_hist, r_hist; cv::Mat b_blur, g_blur, r_blur; cv::Mat y_hist, cr_hist, cb_hist; cv::Mat y_blur, cr_blur, cb_blur; GaussianBlur(bgr_planes[0], b_blur, cv::Size(5,5), 0, 0); GaussianBlur(bgr_planes[1], g_blur, cv::Size(5,5), 0, 0); GaussianBlur(bgr_planes[2], r_blur, cv::Size(5,5), 0, 0); GaussianBlur(ycbcr_planes[0], y_blur, cv::Size(5,5), 0, 0); GaussianBlur(ycbcr_planes[1], cr_blur, cv::Size(5,5), 0, 0); GaussianBlur(ycbcr_planes[2], cb_blur, cv::Size(5,5), 0, 0); /// Compute the histograms: calcHist( &b_blur, 1, 0, cv::Mat(), b_hist, 1, &histSize, &histRange, uniform, accumulate ); calcHist( &g_blur, 1, 0, cv::Mat(), g_hist, 1, &histSize, &histRange, uniform, accumulate ); calcHist( &r_blur, 1, 0, cv::Mat(), r_hist, 1, &histSize, &histRange, uniform, accumulate ); calcHist( &y_blur, 1, 0, cv::Mat(), y_hist, 1, &histSize, &histRange, uniform, accumulate ); calcHist( &cr_blur, 1, 0, cv::Mat(), cr_hist, 1, &histSize, &histRange, uniform, accumulate ); calcHist( &cb_blur, 1, 0, cv::Mat(), cb_hist, 1, &histSize, &histRange, uniform, accumulate ); /// Normalize the result to [ 0, 100 ], percentage representation,, y-axis // for example, there are N blue pixels at 150 [0, 255], and N is the max // from [0, 255], then N is max of the range:100 normalize(b_hist, b_hist, 0, 100, cv::NORM_MINMAX, -1, cv::Mat() ); normalize(g_hist, g_hist, 0, 100, cv::NORM_MINMAX, -1, cv::Mat() ); normalize(r_hist, r_hist, 0, 100, cv::NORM_MINMAX, -1, cv::Mat() ); normalize(y_hist, y_hist, 0, 100, cv::NORM_MINMAX, -1, cv::Mat() ); normalize(cr_hist, cr_hist, 0, 100, cv::NORM_MINMAX, -1, cv::Mat() ); normalize(cb_hist, cb_hist, 0, 100, cv::NORM_MINMAX, -1, cv::Mat() ); // clear vector first pHistVector_->clear(); pHistVector_->push_back(cr_hist); pHistVector_->push_back(cb_hist); return pHistVector_; }