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