Point3i World::GetRayCollisionDist(const Vec3f &start, const Vec3f &dir, float castDist) // Returns -1.0f of the ray didn't hit anything
{
float dist = 1.6f;
// Test every block length, so will hit all blocks no matter what
while(dist <= castDist)
{
// Get the position along the ray
Vec3f pos(start + dir * dist);
// Get the voxel at this position
int vx = static_cast<int>(pos.x);
int vy = static_cast<int>(pos.y);
int vz = static_cast<int>(pos.z);
unsigned char type = GetVoxel(vx, vy, vz);
// If solid, stop testing
if(type != 0)
return Point3i(vx, vy, vz);
dist += 1.0f;
}
// Didn't hit anything, return may power
return Point3i(-1, -1, -1);
}
Point3i BaseFlow::like_ransac(vector<Point3i> vec)
{
Point2i center_init,center_good;
float sum_x=0,sum_y=0;
for (size_t i = 0; i < vec.size(); i++)
{
sum_x += vec[i].x;
sum_y += vec[i].y;
}
center_init.x = sum_x/vec.size();
center_init.y = sum_y/vec.size();
//cout<<"center_init.x1: "<<center_init.x<<" "<<"center_init.y1: "<<center_init.y<<endl;
std::sort(vec.begin(),vec.end(),comp); //upsort
sum_x=0;sum_y=0;
for (size_t i = 0; i < vec.size()/2; i++)
{
sum_x += vec[i].x;
sum_y += vec[i].y;
}
center_init.x = sum_x/(vec.size()/2);
center_init.y = sum_y/(vec.size()/2);
//cout<<"center_init.x2: "<<center_init.x<<" "<<"center_init.y2: "<<center_init.y<<endl;
return Point3i(center_init.x,center_init.y,0);
}
Mat FastLSPredictionComputer::getBuffer(const Point3i& currentPos) {
int pos[] = {currentPos.z - context->getFullNeighborhood().getFront(), currentPos.y - context->getFullNeighborhood().getTop(),
currentPos.x - context->getFullNeighborhood().getLeft(), 0, 0}; // buffer position
if(pos[0] < 0 || pos[1] < 0 || pos[2] < 0 || pos[2] >= covMatBuffer.size[2] ||
pos[1] >= context->getImage()->size[1] - context->getFullNeighborhood().getMask().size[1] + 1)
return zeroBuffer; // outside the buffer return zero matrix
if(pos[0] >= covMatBuffer.size[0]) { // slice ringbuffer is active
if(pos[0] > (int)currentSlice) { // rotate ringbuffer
pos[0] %= covMatBuffer.size[0]; // ringbuffer position
int startSlice[] = {pos[0], 0, 0, 0, 0};
for(double *nanPtr = &(covMatBuffer.at<double>(startSlice)),
*endPtr = nanPtr + covMatBuffer.size[1] * covMatBuffer.size[2] * covMatBuffer.size[3] * covMatBuffer.size[4];
nanPtr < endPtr; nanPtr += covMatBuffer.size[3] * covMatBuffer.size[4])
*nanPtr = numeric_limits<double>::quiet_NaN(); // set upper left matrix values to nan
++currentSlice;
currentRow = context->getTrainingregion().getTop() + 1;
} else pos[0] %= covMatBuffer.size[0];
}
if(!context->getTrainingregion().getFront()) { // row ringbuffer is active
if(pos[1] > (int)currentRow) { // rotate ringbuffer
pos[1] %= covMatBuffer.size[1]; // ringbuffer position
int startRow[] = {0, pos[1], 0, 0, 0};
for(double *nanPtr = &(covMatBuffer.at<double>(startRow)),
*endPtr = nanPtr + covMatBuffer.size[2] * covMatBuffer.size[3] * covMatBuffer.size[4];
nanPtr < endPtr; nanPtr += covMatBuffer.size[3] * covMatBuffer.size[4])
*nanPtr = numeric_limits<double>::quiet_NaN(); // set upper left matrix values to nan
++currentRow;
} else pos[1] %= covMatBuffer.size[1];
}
double* bufPtr = &(covMatBuffer.at<double>(pos));
#ifdef WIN32
if(_isnan(*bufPtr)) {
#else
if(isnan(*bufPtr)) {
#endif
double* currentBufPtr = bufPtr;
const double* leftBufPtr = getBuffer(currentPos + Point3i(-1, 0, 0)).ptr<double>();
const double* topBufPtr = getBuffer(currentPos + Point3i( 0, -1, 0)).ptr<double>();
const double* topleftBufPtr = getBuffer(currentPos + Point3i(-1, -1, 0)).ptr<double>();
Mat sampleVector;
context->contextOf(currentPos, sampleVector);
const double* const sampleVectorPtr = sampleVector.ptr<double>();
for(int k = 0; k < sampleVector.cols; ++k) // matrix is continuous!
for(int l = 0; l < sampleVector.cols; ++l)
*(currentBufPtr++) = sampleVectorPtr[k] * sampleVectorPtr[l] + *(topBufPtr++) + *(leftBufPtr++) - *(topleftBufPtr++);
if(context->getTrainingregion().getFront()) { // 3-D training region
const double* frontBufPtr = getBuffer(currentPos + Point3i( 0, 0, -1)).ptr<double>();
const double* frontleftBufPtr = getBuffer(currentPos + Point3i(-1, 0, -1)).ptr<double>();
const double* fronttopBufPtr = getBuffer(currentPos + Point3i( 0, -1, -1)).ptr<double>();
const double* fronttopleftBufPtr = getBuffer(currentPos + Point3i(-1, -1, -1)).ptr<double>();
currentBufPtr = bufPtr;
for(int k = 0; k < sampleVector.cols; ++k)
for(int l = 0; l < sampleVector.cols; ++l)
*(currentBufPtr++) += *(frontBufPtr++) - *(frontleftBufPtr++) - *(fronttopBufPtr++) + *(fronttopleftBufPtr++);
}
}
return Mat(context->getFullNeighborhood().getNumberOfElements(), context->getFullNeighborhood().getNumberOfElements(), CV_64F, bufPtr);
} // end FastLSPredictionComputer::updateBuffer
} // end namespace vanilc
void GPUTexture::setBitmap(unsigned int slot, Bitmap *bitmap) {
while (slot >= m_bitmaps.size())
m_bitmaps.push_back(NULL);
if (slot == 0 && bitmap != NULL) {
m_size = Point3i(
bitmap->getWidth(),
bitmap->getHeight(), 1);
if (bitmap->getWidth() == 1 || bitmap->getHeight() == 1)
m_type = ETexture1D;
else
m_type = ETexture2D;
switch (bitmap->getPixelFormat()) {
case Bitmap::ELuminance: m_pixelFormat = ELuminance; break;
case Bitmap::ELuminanceAlpha: m_pixelFormat = ELuminanceAlpha; break;
case Bitmap::ERGB: m_pixelFormat = ERGB; break;
case Bitmap::ERGBA: m_pixelFormat = ERGBA; break;
#if SPECTRUM_SAMPLES == 3
case Bitmap::ESpectrum: m_pixelFormat = ERGB; break;
case Bitmap::ESpectrumAlpha: m_pixelFormat = ERGBA; break;
#endif
default:
Log(EError, "Unsupported pixel format %i!",
(int) bitmap->getPixelFormat());
}
switch (bitmap->getComponentFormat()) {
case Bitmap::EUInt8: m_componentFormat = EUInt8; break;
case Bitmap::EUInt16: m_componentFormat = EUInt16; break;
case Bitmap::EUInt32: m_componentFormat = EUInt32; break;
case Bitmap::EFloat16: m_componentFormat = EFloat16; break;
case Bitmap::EFloat32: m_componentFormat = EFloat32; break;
case Bitmap::EFloat64: m_componentFormat = EFloat64; break;
default:
Log(EError, "Unsupported component format %i!",
(int) bitmap->getComponentFormat());
}
}
if (m_bitmaps[slot] != NULL)
m_bitmaps[slot]->decRef();
m_bitmaps[slot] = bitmap;
if (bitmap != NULL)
bitmap->incRef();
}
vector< Point3i > FireflyOptimizator::moveToward(vector<pair<Point2i, Point2i>> vec,float amount)
{
vector< Point3i > markerC2;
for( pair<Point2i, Point2i> p : vec)
{
p.second = interpolate(p,amount);
markerC2.push_back(Point3i(p.second.x,p.second.y,0));
}
//WatershedImage r(imagePath,markerC2);
return markerC2;
}
Point3i BaseFlow::weng_method(vector<Point3i> vec)
{
float min_dis=600000;
int index; //stack if not init,it will be unknow
for (size_t i = 0; i < vec.size(); i++)
{
float sum=0;
for (size_t j = 0; j < vec.size(); j++)
{
if(i != j)
sum += abs(vec[i].x - vec[j].x) + abs(vec[i].y - vec[j].y);
}
if(sum < min_dis)
{
min_dis = sum;
index = i;
}
}
return Point3i(vec[index].x,vec[index].y,index);
}
//圆拟合函数，性能很好，但要防止拟合时出现奇异值的情况
bool findCircleParameter::CircleFitByKasa(vector<Point> validPoints, Point& center, int& radius)
{
if (validPoints.size() <= 2)
{
cout << "The Circle fit failed, Because there is not enought validate points to use!" << endl;
return false;
}
vector<Point3i> extendA;
vector<int> extendB;
vector<Point>::iterator iter = validPoints.begin();
while (iter != validPoints.end())
{
extendA.push_back(Point3i((*iter).x, (*iter).y, 1));
extendB.push_back((pow((*iter).x, 2) + pow((*iter).y, 2)));
iter++;
}
Mat A = Mat(extendA).reshape(1);
Mat B = Mat(extendB).reshape(1);
Mat_<double> dA, dB;
Mat_<double> P(3, 1, CV_64F);
A.convertTo(dA, CV_64F);
B.convertTo(dB, CV_64F);
P = dA.inv(CV_SVD)*dB;
//cout << P << endl;
double p1, p2, p3;
p1 = P.at<double>(0, 0);
p2 = P.at<double>(1, 0);
p3 = P.at<double>(2, 0);
center.x = p1 / 2;
center.y = p2 / 2;
radius = sqrt((pow(p1, 2) + pow(p2, 2)) / 4 + p3);
//cout << center.x << endl << center.y << endl << radius << endl;
return true;
}
MTS_NAMESPACE_BEGIN
GPUTexture::GPUTexture(const std::string &name, Bitmap *bitmap)
: m_name(name) {
m_filterType = EMipMapLinear;
m_wrapTypeU = m_wrapTypeV = EClampToEdge;
m_mipmapped = true;
m_maxAnisotropy = 0.0f;
m_fbType = ENone;
m_samples = 1;
m_depthMode = ECompare;
m_borderColor = Color3(static_cast<Float>(0));
m_size = Point3i(0);
if (bitmap != NULL) {
setBitmap(0, bitmap);
} else {
m_type = ETexture2D;
m_pixelFormat = ERGB;
m_componentFormat = EUInt8;
}
}
void VPLShaderManager::setVPL(const VPL &vpl) {
Point p = vpl.its.p + vpl.its.shFrame.n * 0.01;
Intersection its;
/* Estimate good near and far plane locations by tracing some rays */
Float nearClip = std::numeric_limits<Float>::infinity(),
farClip = -std::numeric_limits<Float>::infinity();
Ray ray;
ray.o = p;
if (m_shadowMap == NULL || m_shadowMapResolution != m_shadowMap->getSize().x) {
m_shadowMap = m_renderer->createGPUTexture("Shadow cube map", NULL);
m_shadowMap->setSize(Point3i(m_shadowMapResolution, m_shadowMapResolution, 1));
m_shadowMap->setFrameBufferType(GPUTexture::EDepthBuffer);
m_shadowMap->setType(GPUTexture::ETextureCubeMap);
m_shadowMap->setWrapType(GPUTexture::EClampToEdge);
m_shadowMap->setFilterType(GPUTexture::ENearest);
m_shadowMap->setDepthMode(GPUTexture::ENormal);
m_shadowMap->init();
}
const int sampleCount = 200;
const Float invSampleCount = 1.0f/sampleCount;
for (int i=1; i<=sampleCount; ++i) {
Vector dir;
Point2 seed(i*invSampleCount, radicalInverse(2, i)); // Hammersley seq.
if (vpl.type == ESurfaceVPL || vpl.luminaire->getType() & Luminaire::EOnSurface)
dir = vpl.its.shFrame.toWorld(squareToHemispherePSA(seed));
else
dir = squareToSphere(seed);
ray.setDirection(dir);
if (m_scene->rayIntersect(ray, its)) {
nearClip = std::min(nearClip, its.t);
farClip = std::max(farClip, its.t);
}
}
m_minDist = nearClip + (farClip - nearClip) * m_clamping;
nearClip = std::min(nearClip, (Float) 0.001f);
farClip = std::min(farClip * 1.5f, m_maxClipDist);
if (farClip < 0 || nearClip >= farClip) {
/* Unable to find any surface - just default values based on the scene size */
nearClip = 1e-3f * m_scene->getBSphere().radius;
farClip = 2 * m_scene->getBSphere().radius;
m_minDist = 0;
}
farClip = std::min(farClip, 5.0f*m_scene->getBSphere().radius);
m_nearClip = nearClip;
m_invClipRange = 1/(farClip-nearClip);
Transform lightViewTrafo, lightProjTrafo = Transform::glPerspective(90.0f, nearClip, farClip);
Matrix4x4 identity;
identity.setIdentity();
m_renderer->setCamera(identity, identity);
m_shadowMap->activateTarget();
if (m_singlePass && m_shadowProgram != NULL) {
/* "Fancy": render the whole cube map in a single pass using
a geometry program. On anything but brand-new hardware, this
is actually slower. */
m_shadowMap->activateSide(-1);
m_shadowMap->clear();
m_shadowProgram->bind();
try {
for (int i=0; i<6; ++i) {
switch (i) {
case 0: lightViewTrafo = Transform::lookAt(p, p + Vector(1, 0, 0), Vector(0, 1, 0)).inverse(); break;
case 1: lightViewTrafo = Transform::lookAt(p, p + Vector(-1, 0, 0), Vector(0, 1, 0)).inverse(); break;
case 2: lightViewTrafo = Transform::lookAt(p, p + Vector(0, 1, 0), Vector(0, 0, -1)).inverse(); break;
case 3: lightViewTrafo = Transform::lookAt(p, p + Vector(0, -1, 0), Vector(0, 0, 1)).inverse(); break;
case 4: lightViewTrafo = Transform::lookAt(p, p + Vector(0, 0, 1), Vector(0, 1, 0)).inverse(); break;
case 5: lightViewTrafo = Transform::lookAt(p, p + Vector(0, 0, -1), Vector(0, 1, 0)).inverse(); break;
}
lightViewTrafo = Transform::scale(Vector(-1, 1, 1)) * lightViewTrafo;
const Matrix4x4 &viewMatrix = lightViewTrafo.getMatrix();
m_shadowProgram->setParameter(m_shadowProgramParam_cubeMapTransform[i], lightProjTrafo * lightViewTrafo);
m_shadowProgram->setParameter(m_shadowProgramParam_depthVec[i], Vector4(
-viewMatrix.m[2][0] * m_invClipRange,
-viewMatrix.m[2][1] * m_invClipRange,
-viewMatrix.m[2][2] * m_invClipRange,
(-viewMatrix.m[2][3] - m_nearClip) * m_invClipRange
));
}
m_renderer->drawAll(m_drawList);
} catch (const std::exception &ex) {
m_shadowProgram->unbind();
throw ex;
}
m_shadowProgram->unbind();
} else {
/* Old-fashioned: render 6 times, once for each cube map face */
m_altShadowProgram->bind();
try {
for (int i=0; i<6; ++i) {
switch (i) {
case 0: lightViewTrafo = Transform::lookAt(p, p + Vector(1, 0, 0), Vector(0, 1, 0)).inverse(); break;
case 1: lightViewTrafo = Transform::lookAt(p, p + Vector(-1, 0, 0), Vector(0, 1, 0)).inverse(); break;
case 2: lightViewTrafo = Transform::lookAt(p, p + Vector(0, 1, 0), Vector(0, 0, -1)).inverse(); break;
case 3: lightViewTrafo = Transform::lookAt(p, p + Vector(0, -1, 0), Vector(0, 0, 1)).inverse(); break;
case 4: lightViewTrafo = Transform::lookAt(p, p + Vector(0, 0, 1), Vector(0, 1, 0)).inverse(); break;
case 5: lightViewTrafo = Transform::lookAt(p, p + Vector(0, 0, -1), Vector(0, 1, 0)).inverse(); break;
}
lightViewTrafo = Transform::scale(Vector(-1, 1, 1)) * lightViewTrafo;
const Matrix4x4 &viewMatrix = lightViewTrafo.getMatrix();
m_altShadowProgram->setParameter(m_altShadowProgramParam_cubeMapTransform, lightProjTrafo * lightViewTrafo);
m_altShadowProgram->setParameter(m_altShadowProgramParam_depthVec, Vector4(
-viewMatrix.m[2][0] * m_invClipRange,
-viewMatrix.m[2][1] * m_invClipRange,
-viewMatrix.m[2][2] * m_invClipRange,
(-viewMatrix.m[2][3] - m_nearClip) * m_invClipRange
));
m_shadowMap->activateSide(i);
m_shadowMap->clear();
m_renderer->drawAll(m_drawList);
}
} catch (std::exception &ex) {
m_altShadowProgram->unbind();
throw ex;
}
m_altShadowProgram->unbind();
}
m_shadowMap->releaseTarget();
}
void PushbroomStereo::RunStereoPushbroomStereo( Mat leftImage, Mat rightImage, Mat laplacian_left, Mat laplacian_right,
cv::vector<Point3f> *pointVector3d, cv::vector<Point3i> *pointVector2d, cv::vector<uchar> *pointColors,
int row_start, int row_end, PushbroomStereoState state )
{
// we will do this by looping through every block in the left image
// (defined by blockSize) and checking for a matching value on
// the right image
cv::vector<Point3f> localHitPoints;
int blockSize = state.blockSize;
int disparity = state.disparity;
int sadThreshold = state.sadThreshold;
int startJ = 0;
int stopJ = leftImage.cols - (disparity + blockSize);
if (disparity < 0)
{
startJ = -disparity;
stopJ = leftImage.cols - blockSize;
}
//printf("row_start: %d, row_end: %d, startJ: %d, stopJ: %d, rows: %d, cols: %d\n", row_start, row_end, startJ, stopJ, leftImage.rows, leftImage.cols);
int hitCounter = 0;
if (state.random_results < 0) {
int *sadArray = new int[ leftImage.rows * leftImage.step ];
int iStep, jStep;
#ifdef USE_GPU
StopWatchInterface *timer;
sdkCreateTimer( &timer );
sdkResetTimer( &timer );
sdkStartTimer( &timer );
//GetSADBlock(row_start, row_end, blockSize, startJ, stopJ, sadArray, leftImage, rightImage, laplacian_left, laplacian_right, state);
m_sadCalculator.runGetSAD( row_start, row_end, startJ, stopJ, sadArray, leftImage.data, rightImage.data, laplacian_left.data, laplacian_right.data, leftImage.step,
state.blockSize, state.disparity, state.sobelLimit );
sdkStopTimer( &timer );
//printf("RunStereo bottleneck timer: %.2f ms \n", sdkGetTimerValue( &timer) );
sdkDeleteTimer( &timer );
#endif
int gridY = (row_end - row_start)/blockSize;
int gridX = (stopJ - startJ)/blockSize;
for (int y=0; y< gridY; y++)
{
for (int x=0; x< gridX; x++)
{
// check to see if the SAD is below the threshold,
// indicating a hit
int i = row_start + y * blockSize;
int j = startJ + x * blockSize;
#ifdef USE_GPU
int sad = sadArray[ y * gridX + x];
#else
int sad= GetSAD(leftImage, rightImage, laplacian_left, laplacian_right, j, i, state);
#endif
if (sad < sadThreshold && sad >= 0)
{
// got a hit
// now check for horizontal invariance
// (ie check for parts of the image that look the same as this
// which would indicate that this might be a false-positive)
if (!state.check_horizontal_invariance || CheckHorizontalInvariance(leftImage, rightImage, laplacian_left, laplacian_right, j, i, state) == false) {
// add it to the vector of matches
// don't forget to offset it by the blockSize,
// so we match the center of the block instead
// of the top left corner
localHitPoints.push_back(Point3f(j+blockSize/2.0, i+blockSize/2.0, -disparity));
//localHitPoints.push_back(Point3f(state.debugJ, state.debugI, -disparity));
uchar pxL = leftImage.at<uchar>(i,j);
pointColors->push_back(pxL); // TODO: this is the corner of the box, not the center
hitCounter ++;
if (state.show_display)
{
pointVector2d->push_back(Point3i(j, i, sad));
}
} // check horizontal invariance
}
}
}
} else {
double intpart;
float fractpart = modf(state.random_results , &intpart);
hitCounter = int(intpart);
// determine if this is a time we'll use that last point
std::random_device rd;
std::default_random_engine generator(rd()); // rd() provides a random seed
std::uniform_real_distribution<float> distribution(0, 1);
if (fractpart > distribution(generator)) {
hitCounter ++;
}
for (int i = 0; i < hitCounter; i++) {
int randx = rand() % (stopJ - startJ) + startJ;
int randy = rand() % (row_end - row_start) + row_start;
localHitPoints.push_back(Point3f(randx, randy, -disparity));
}
}
// now we have an array of hits -- transform them to 3d points
if (hitCounter > 0) {
perspectiveTransform(localHitPoints, *pointVector3d, state.Q);
}
}
World::World()
: m_created(false),
m_lightVec(0.4f, -0.5f, -0.7f),
m_ambient(0.1f),
m_pPhysicsWorld(NULL), m_pathFindingThreadPool(8), m_AOSampleDistance(6.0f),
m_AOSampleAngle(pif / s_numAOSamples)
{
m_lightVec = m_lightVec.Normalize();
// Generate normals
m_normals[0] = Vec3f(1.0f, 0.0f, 0.0f);
m_normals[1] = Vec3f(-1.0f, 0.0f, 0.0f);
m_normals[2] = Vec3f(0.0f, 1.0f, 0.0f);
m_normals[3] = Vec3f(0.0f, -1.0f, 0.0f);
m_normals[4] = Vec3f(0.0f, 0.0f, 1.0f);
m_normals[5] = Vec3f(0.0f, 0.0f, -1.0f);
// Generate test offsets
m_positionOffsets[0] = Point3i(1, 0, 0);
m_positionOffsets[1] = Point3i(-1, 0, 0);
m_positionOffsets[2] = Point3i(0, 1, 0);
m_positionOffsets[3] = Point3i(0, -1, 0);
m_positionOffsets[4] = Point3i(0, 0, 1);
m_positionOffsets[5] = Point3i(0, 0, -1);
// Generate corners
float cornerDist = 0.5f;
m_corners[0][0] = Vec3f(cornerDist, -cornerDist, cornerDist);
m_corners[0][1] = Vec3f(cornerDist, -cornerDist, -cornerDist);
m_corners[0][2] = Vec3f(cornerDist, cornerDist, -cornerDist);
m_corners[0][3] = Vec3f(cornerDist, cornerDist, cornerDist);
m_corners[1][0] = Vec3f(-cornerDist, -cornerDist, -cornerDist);
m_corners[1][1] = Vec3f(-cornerDist, -cornerDist, cornerDist);
m_corners[1][2] = Vec3f(-cornerDist, cornerDist, cornerDist);
m_corners[1][3] = Vec3f(-cornerDist, cornerDist, -cornerDist);
m_corners[2][0] = Vec3f(-cornerDist, cornerDist, -cornerDist);
m_corners[2][1] = Vec3f(-cornerDist, cornerDist, cornerDist);
m_corners[2][2] = Vec3f(cornerDist, cornerDist, cornerDist);
m_corners[2][3] = Vec3f(cornerDist, cornerDist, -cornerDist);
m_corners[3][0] = Vec3f(-cornerDist, -cornerDist, cornerDist);
m_corners[3][1] = Vec3f(-cornerDist, -cornerDist, -cornerDist);
m_corners[3][2] = Vec3f(cornerDist, -cornerDist, -cornerDist);
m_corners[3][3] = Vec3f(cornerDist, -cornerDist, cornerDist);
m_corners[4][0] = Vec3f(-cornerDist, -cornerDist, cornerDist);
m_corners[4][1] = Vec3f(cornerDist, -cornerDist, cornerDist);
m_corners[4][2] = Vec3f(cornerDist, cornerDist, cornerDist);
m_corners[4][3] = Vec3f(-cornerDist, cornerDist, cornerDist);
m_corners[5][0] = Vec3f(cornerDist, -cornerDist, -cornerDist);
m_corners[5][1] = Vec3f(-cornerDist, -cornerDist, -cornerDist);
m_corners[5][2] = Vec3f(-cornerDist, cornerDist, -cornerDist);
m_corners[5][3] = Vec3f(cornerDist, cornerDist, -cornerDist);
// Generate chunk occlusion query
glGenQueriesARB(1, &m_chunkOcclusionQueryID);
GL_ERROR_CHECK();
m_unmanagedName = "world";
GetSphereDistribution(m_sphereDistribution);
}
void LzCalculator::center_match(vector<vector<double>> &L_points,vector<vector<double>> &R_points,vector<double>& L_opt_ps,vector<double>& R_opt_ps)
{
int _L_index[2000]={0}; //0表示该行没有点，1表示该行唯一点，-1表示该行多点！
int _R_index[2000]={0};
int seed_begin = 0; //用于计数填充种子的起始和结束行号。//
int seed_end = 0;
float center_L,center_R;
bool is_calc = false;
int seed_cunt = 0;
Point3i seed;
single_seed.push_back(Point3i(0,0,0));
for(int i=0;i<L_opt_ps.size();i++)
{
/* if(R_points[i].size()==1&&L_points[i].size()==1)
{
_L_index[i] = 1;
L_opt_ps[i] = L_points[i][0];
// record[i] = 1;
}
else if(L_points[i].size()==0)
{
_L_index[i] = 0;
}
else
{
_L_index[i] = -1;
}
if(R_points[i].size()==1&&L_points[i].size()==1)
{
_R_index[i] = 1;
R_opt_ps[i] = R_points[i][0];
}
else if(R_points[i].size()==0)
{
_R_index[i] = 0;
}
else
{
_R_index[i] = -1;
}*/
if(L_points[i].size()==1&&R_points[i].size()==1&&is_calc==false)
{
if(is_calc==false)
{
is_calc = true;
seed_begin = i;
}
seed_cunt++;
}
else if(is_calc==true&&L_points[i].size()==1&&R_points[i].size()==1&&abs(L_points[i][0]-L_points[i-1][0])<0.5&&abs(R_points[i][0]-R_points[i-1][0])<0.5)
{
seed_cunt++;
// std::cout<<i<<endl;
}
else
{
if(is_calc==true)
{
if(30<seed_cunt)
{
seed_end = i;
seed.x = seed_begin;
seed.y = seed_end;
seed.z = seed_cunt;
single_seed.push_back(seed);
}
// std::cout<<seed_cunt<<endl;
seed_cunt = 0;
is_calc = false;
}
}
}
single_seed.push_back(Point3i(L_points.size(),-1,0));
/* for(int i=0;i<L_opt_ps.size();i++)
{
int min = 10000;
int min_index = 0;
if((_R_index[i]==1&&_L_index[i]==1)||_R_index[i]==0||_L_index[i]==0)
continue;
else
{
if((0<i)&&(_L_index[i]==-1&&(_L_index[i-1])!=0))
{
for(int j=0;j<L_points[i].size();j++)
{
if(L_points[i][j]-L_points[i-1][0]<min)
{
min = L_points[i][j]-L_points[i-1][0];
min_index = j;
}
}
L_opt_ps[i] = L_points[i][min_index];
}
min = 1000;
if((0<i)&&(_R_index[i]==-1)&&(_R_index[i-1]!=0))
{
for(int j=0;j<R_points[i].size();j++)
{
if(R_points[i][j]-R_points[i-1][0]<min)
{
min = R_points[i][j]-R_points[i-1][0];
min_index = j;
}
}
R_opt_ps[i] = R_points[i][min_index];
}
}
}*/
if(2<single_seed.size())
{
for(int i=1;i<single_seed.size()-1;i++)
{
for(int j=single_seed[i].x;j<single_seed[i].y;j++)
{
L_opt_ps[j] = L_points[j][0];
R_opt_ps[j] = R_points[j][0];
record_L[j] = 1;
record_R[j] = 1;
}
}
seed_fill(single_seed);
}
pnts_add(); //加入图像中两条光带的不确定点对。
pnts_rough_add(); //加入粗提取的当光带。
}
bool World::Create(int chunksInX, int chunksInY, int chunksInZ, const std::string &diffuseTextureFilePath, const std::string &specularTextureFilePath, const std::string &bumpTextureFilePath, const std::string &textureDescFilePath)
{
assert(!m_created);
// Load the voxel textures
if(!m_voxelTex_diffuse.LoadAsset(diffuseTextureFilePath))
{
std::cerr << "Failed to create world: Could not load " << diffuseTextureFilePath << "!" << std::endl;
return false;
}
if(!m_voxelTex_specular.LoadAsset(specularTextureFilePath))
{
std::cerr << "Failed to create world: Could not load " << specularTextureFilePath << "!" << std::endl;
return false;
}
if(!m_voxelTex_bump.LoadAsset(bumpTextureFilePath))
{
std::cerr << "Failed to create world: Could not load " << bumpTextureFilePath << "!" << std::endl;
return false;
}
// Load the voxel texture description file
if(!LoadVoxelFaceTextureDesc(textureDescFilePath))
{
std::cerr << "Failed to create world: Could not load " << textureDescFilePath << "!" << std::endl;
return false;
}
m_chunksInX = chunksInX;
m_chunksInY = chunksInY;
m_chunksInZ = chunksInZ;
m_aabb = AABB(Vec3f(0.0f, 0.0f, 0.0f), Vec3f(static_cast<float>(m_chunksInX * Chunk::s_chunkSizeX), static_cast<float>(m_chunksInY * Chunk::s_chunkSizeY), static_cast<float>(m_chunksInZ * Chunk::s_chunkSizeZ)));
// Resize the chunk matrix
m_chunkMatrix.resize(m_chunksInX);
for(int x = 0; x < m_chunksInX; x++)
{
m_chunkMatrix[x].resize(m_chunksInY);
for(int y = 0; y < m_chunksInY; y++)
{
m_chunkMatrix[x][y].resize(m_chunksInZ);
for(int z = 0; z < m_chunksInZ; z++)
m_chunkMatrix[x][y][z].Create(this, Point3i(x, y, z));
}
}
float maxA;
glGetFloatv(GL_MAX_TEXTURE_MAX_ANISOTROPY_EXT, &maxA);
m_voxelTex_diffuse.GenMipMaps();
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAX_ANISOTROPY_EXT, maxA);
m_voxelTex_specular.GenMipMaps();
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAX_ANISOTROPY_EXT, maxA);
m_voxelTex_bump.GenMipMaps();
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAX_ANISOTROPY_EXT, maxA);
m_created = true;
return true;
}
bool render(Scene *scene, RenderQueue *queue,
const RenderJob *job, int sceneResID, int sensorResID, int samplerResID) {
ref<Sensor> sensor = scene->getSensor();
ref<Film> film = sensor->getFilm();
m_cancel = false;
if (!sensor->getClass()->derivesFrom(MTS_CLASS(ProjectiveCamera)))
Log(EError, "The VPL renderer requires a projective camera!");
/* Initialize hardware rendering */
m_framebuffer = m_renderer->createGPUTexture("Framebuffer", NULL);
m_framebuffer->setFrameBufferType(GPUTexture::EColorBuffer);
m_framebuffer->setComponentFormat(GPUTexture::EFloat32);
m_framebuffer->setPixelFormat(GPUTexture::ERGB);
m_framebuffer->setSize(Point3i(film->getSize().x, film->getSize().y, 1));
m_framebuffer->setFilterType(GPUTexture::ENearest);
m_framebuffer->setMipMapped(false);
m_accumBuffer = m_renderer->createGPUTexture("Accumulation buffer",
new Bitmap(Bitmap::ERGB, Bitmap::EFloat32, film->getSize()));
m_accumBuffer->setFrameBufferType(GPUTexture::EColorBuffer);
m_framebuffer->setComponentFormat(GPUTexture::EFloat32);
m_framebuffer->setPixelFormat(GPUTexture::ERGB);
m_accumBuffer->setMipMapped(false);
MTS_AUTORELEASE_BEGIN()
m_session->init();
m_device->setSize(film->getSize());
m_device->init();
m_device->setVisible(false);
m_renderer->init(m_device);
if (!m_renderer->getCapabilities()->isSupported(
RendererCapabilities::EShadingLanguage))
Log(EError, "Support for GLSL is required!");
if (!m_renderer->getCapabilities()->isSupported(
RendererCapabilities::ERenderToTexture))
Log(EError, "Render-to-texture support is required!");
if (!m_renderer->getCapabilities()->isSupported(
RendererCapabilities::EFloatingPointTextures))
Log(EError, "Floating point texture support is required!");
if (!m_renderer->getCapabilities()->isSupported(
RendererCapabilities::EFloatingPointBuffer))
Log(EError, "Floating point render buffer support is required!");
if (!m_renderer->getCapabilities()->isSupported(
RendererCapabilities::EVertexBufferObjects))
Log(EError, "Vertex buffer object support is required!");
if (!m_renderer->getCapabilities()->isSupported(
RendererCapabilities::EGeometryShaders))
Log(EError, "Geometry shader support is required!");
/* Initialize and clear the framebuffer */
m_framebuffer->init();
m_accumBuffer->init();
m_accumBuffer->activateTarget();
m_accumBuffer->clear();
m_accumBuffer->releaseTarget();
m_shaderManager = new VPLShaderManager(m_renderer);
m_shaderManager->setShadowMapResolution(m_shadowMapResolution);
m_shaderManager->setClamping(m_clamping);
m_shaderManager->init();
m_shaderManager->setScene(scene);
ProgressReporter progress("Rendering", m_vpls.size(), job);
for (size_t i=0; i<m_vpls.size() && !m_cancel; ++i) {
const VPL &vpl = m_vpls[i];
m_renderer->setDepthMask(true);
m_renderer->setDepthTest(true);
m_renderer->setBlendMode(Renderer::EBlendNone);
m_shaderManager->setVPL(vpl);
m_framebuffer->activateTarget();
m_framebuffer->clear();
drawShadowedScene(scene, vpl);
m_framebuffer->releaseTarget();
m_renderer->setDepthMask(false);
m_renderer->setDepthTest(false);
m_renderer->setBlendMode(Renderer::EBlendAdditive);
m_accumBuffer->activateTarget();
m_renderer->blitTexture(m_framebuffer, true);
m_accumBuffer->releaseTarget();
if ((i%20) == 0) {
m_renderer->flush();
m_renderer->checkError();
}
progress.update(i);
}
progress.finish();
m_accumBuffer->download();
film->setBitmap(m_accumBuffer->getBitmap());
m_shaderManager->cleanup();
m_shaderManager = NULL;
m_framebuffer->cleanup();
m_accumBuffer->cleanup();
m_renderer->shutdown();
m_device->shutdown();
m_session->shutdown();
MTS_AUTORELEASE_END()
return !m_cancel;
}
void QRFinder::FindFinderPatterns(cv::Mat& inputImg, Rect regionOfInterest, vector<FinderPattern*> & finderPatterns, vector<Drawable*> & debugVector)
{
struct timespec start,end;
SET_TIME(&start);
//Get parameters from config
edgeThreshold = config->GetIntegerParameter("EdgeThreshold");
debugLevel = config->GetIntegerParameter("QR Debug Level");
int verticalResolution = config->GetIntegerParameter("YResolution");
nonMaxEnabled = config->GetBooleanParameter("EdgeNonMax");
minimumFinderPatternScore = config->GetIntegerParameter("MinimumFPScore");
detectorSize = config->GetIntegerParameter("DetectorSize");
int yBorder = detectorSize;
vector<Point3i> exclusionZones;
//Calculate limits
int maxColumn = regionOfInterest.x + regionOfInterest.width;
maxColumn -= detectorSize;
int maxRow = regionOfInterest.y + regionOfInterest.height;
int xStart = regionOfInterest.x;
int yStart = regionOfInterest.y;
xStart += detectorSize;
yStart += detectorSize;
maxColumn -= detectorSize;
maxRow -= detectorSize;
if (debugLevel > 0)
debugVector.push_back(new DebugRectangle(Rect(Point2i(xStart,yStart),Point2i(maxColumn,maxRow)),Colors::Aqua,1));
//Find horizontal edges
SET_TIME(&start);
FindEdgesClosed(inputImg,regionOfInterest,edgeArray,edgeThreshold,nonMaxEnabled,detectorSize);
SET_TIME(&end);
double edgeTime_local = calc_time_double(start,end);
edgeTime = (edgeTime + edgeTime_local)/2.0;
config->SetLabelValue("EdgeTime",(float)edgeTime/1000.0f);
//If debug level set, find all vertical edges and draw them
if (debugLevel <= -2)
{
for (int x = 1; x < verticalEdgeArray.rows-1; x ++)
{
FindEdgesVerticalClosed(inputImg,x);
const short * verticalEdgePtr = verticalEdgeArray.ptr<short>(x);
for (int y = 0; y < (verticalEdgeArray.cols);y++)
{
short transition = verticalEdgePtr[y];
if (transition < 0)
{
debugVector.push_back(new DebugCircle(Point2i(x,y),0,Colors::Fuchsia,true));
}
else if (transition > 0)
{
debugVector.push_back(new DebugCircle(Point2i(x,y),0,Colors::Cyan,true));
}
}
}
}
//END
int bw[6] = { 0 };
int k = 0;
//LOGV(LOGTAG_QR,"ImgSize=[%d,%d] EdgeSize=[%d,%d]",inputImg.rows,inputImg.cols,edgeArray.rows,edgeArray.cols);
int yDirection = 1;
int yCenter = yStart + (maxRow - yStart)/2;
int y = yStart, absOffset = 0;
LOGV(LOGTAG_QR,"Beginning search. y[%d->%d], Center=%d, x[%d->%d]",yStart,maxRow,yCenter,xStart,maxColumn);
while (y < maxRow && y >= yStart)
{
y = yCenter + absOffset * yDirection;
if (yDirection == 1) absOffset += verticalResolution; //Increment every other frame
yDirection = -yDirection; //Change direction every frame
k = 0;
bw[0] = bw[1] = bw[2] = bw[3] = bw[4] = bw[5] = 0;
const short * edgeRowPtr = edgeArray.ptr<short>(y);
for (int x = xStart; x < maxColumn; x++)
{
if (isInRadius(exclusionZones,Point2i(x,y)))
continue;
int transition = edgeRowPtr[x]; //getTransition(Mi,x,threshold);
if (k == 0) //Haven't found edge yet
{
if (transition < 0) /* Light->dark transistion */
{
k++;
}
}
else //Found at least one edge
{
if ((k & 1) == 1) //Counting dark
{
if (transition > 0) //dark to light
{
++k;
}
}
else //Counting light
{
if (transition < 0) //light to dark
{
++k;
}
}
}
if (k > 0) ++bw[k-1];
if (FP_DEBUG_ENABLED && (debugLevel == -1 || debugLevel == -2))
{
if (transition < 0)
debugVector.push_back(new DebugCircle(Point2i(x,y),0,Colors::Lime,true));
else if (transition > 0)
debugVector.push_back(new DebugCircle(Point2i(x,y),0,Colors::Yellow,true));
}
if (k == 6)
{
int result = 0;
result = CheckRatios(bw,NULL);
if (result == 1)
{
//LOGV(LOGTAG_QR,"Ratio check pass");
//Center based on initial ratio
float patternCenterWidth = (float)bw[2];
int tempXCenter = (x - bw[4] - bw[3]) - (int)round(patternCenterWidth/2.0f);
float xOffset = (patternCenterWidth/6.0f);
//y coordinate of center. If check fails, returns 0.
int tempYCenterArray[] = {0,0,0};
int * verticalPatternSizes[3];
for (int i = 0;i < 3;i++)
verticalPatternSizes[i] = new int[5];
tempYCenterArray[0] = FindCenterVertical(inputImg, tempXCenter, y, bw, debugVector,verticalPatternSizes[0]);
tempYCenterArray[1] = FindCenterVertical(inputImg, tempXCenter - xOffset, y, bw, debugVector,verticalPatternSizes[1]);
tempYCenterArray[2] = FindCenterVertical(inputImg, tempXCenter + xOffset, y, bw, debugVector,verticalPatternSizes[2]);
int tempYCenter = 0;
int passCount = 0;
float avgYSize = 0;
int averageVerticalSize[5] = {0,0,0,0,0};
for (int yTest = 0; yTest < 3; yTest++)
{
if (tempYCenterArray[yTest] > 0)
{
passCount++;
tempYCenter += tempYCenterArray[yTest];
for (int i=0;i<5;i++)
{
averageVerticalSize[i] += (verticalPatternSizes[yTest])[i];
avgYSize += (verticalPatternSizes[yTest])[i];
}
}
}
if (passCount >= 2)
{
//LOGV(LOGTAG_QR,"Vertical test pass-1");
tempYCenter = (int)round((float)tempYCenter / (float)passCount);
avgYSize = (float)avgYSize / (float)passCount;
int allowedVariance = (int)avgYSize >> 2;
bool yVarianceTest = true;
for (int yTest = 0; yTest < 3; yTest++)
{
if (tempYCenterArray[yTest] > 0)
{
if (abs(tempYCenterArray[yTest] - tempYCenter) > allowedVariance)
{
yVarianceTest = false;
break;
}
}
}
if (yVarianceTest)
{
//LOGV(LOGTAG_QR,"Vertical test pass-2. Passcount=%d",passCount);
//Average the vertical pattern sizes
for (int i=0;i<5;i++)
{
averageVerticalSize[i] = idiv(averageVerticalSize[i],passCount);
}
//LOGV(LOGTAG_QR,"Averaged sizes. Center=%d",averageVerticalSize[2]);
int tempXCenterArray[] = {0,0,0};
int xSizeArray[] = {0,0,0};
int yOffset = idiv(averageVerticalSize[2],6.0f);
//LOGV(LOGTAG_QR,"Yoffset=%d,yCenter=%d",yOffset,tempYCenter);
tempXCenterArray[0] = FindCenterHorizontal(tempXCenter, tempYCenter-yOffset, bw, xSizeArray[0], debugVector);
tempXCenterArray[1] = FindCenterHorizontal(tempXCenter, tempYCenter, bw, xSizeArray[1], debugVector);
tempXCenterArray[2] = FindCenterHorizontal(tempXCenter, tempYCenter+yOffset, bw, xSizeArray[2], debugVector);
tempXCenter = 0;
passCount = 0;
float avgXSize = 0;
for (int xTest = 0; xTest < 3; xTest++)
{
if (tempXCenterArray[xTest] > 0)
{
passCount++;
tempXCenter += tempXCenterArray[xTest];
avgXSize += xSizeArray[xTest];
}
}
if (passCount >= 2)
{
//LOGV(LOGTAG_QR,"Horizontal test pass");
tempXCenter = (int)round((float)tempXCenter / (float)passCount);
avgXSize = (float)avgXSize/(float)passCount;
//allowedVariance = (int)round((float)avgYSize/1.5f);
float aspectRatio = avgXSize/avgYSize;
if (aspectRatio > 0.33f && aspectRatio < 3.0f)
{
//LOGV(LOGTAG_QR,"Size test pass");
Point2i finderPatternCenter = Point2i(tempXCenter,tempYCenter); //Center of finder pattern
int finderPatternSize = MAX(avgXSize,avgYSize);
int fpRadius = (int)round((float)finderPatternSize/2.0f);
int fpRadiusExclude = ipow(finderPatternSize,2);
//LOGD(LOGTAG_QR,"Creating new pattern[%d,%d]",avgXSize,avgYSize);
//Create a new pattern
FinderPattern * newPattern = new FinderPattern(finderPatternCenter,Size2i(avgXSize,avgYSize));
Size2f patternSearchSize = Size2f(avgXSize,avgYSize);
vector<Point2i> corners;
struct timespec fastStart,fastEnd;
SET_TIME(&fastStart);
fastQRFinder->LocateFPCorners(inputImg,newPattern,corners,debugVector);
// fastQRFinder->CheckAlignmentPattern(inputImg,finderPatternCenter,patternSearchSize,corners,debugVector);
SET_TIME(&fastEnd);
double fastFPTime_Local = calc_time_double(fastStart,fastEnd);
fastFPTime += fastFPTime_Local;
if (corners.size() == 4)
{
//if (validatePattern(newPattern,finderPatterns))
//{
newPattern->patternCorners = corners;
exclusionZones.push_back(Point3i(finderPatternCenter.x,finderPatternCenter.y, fpRadiusExclude));
finderPatterns.push_back(newPattern);
if (FP_DEBUG_ENABLED && debugLevel > 0)
{
debugVector.push_back(new DebugCircle(finderPatternCenter,fpRadius,Colors::MediumSpringGreen,1,true));
for (int i=0;i<corners.size();i++)
{
if (FP_DEBUG_ENABLED && debugLevel > 0)
debugVector.push_back(new DebugCircle(corners[i],10,Colors::DodgerBlue,2));
}
}
//}
//else
//{
// //LOGV(LOGTAG_QR,"Compare check failed");
// if (FP_DEBUG_ENABLED && debugLevel > 0)
// debugVector.push_back(new DebugCircle(finderPatternCenter,fpRadius,Colors::HotPink,2));
//}
}
else
{
//LOGV(LOGTAG_QR,"FAST check failed");
if (FP_DEBUG_ENABLED && debugLevel > 0)
debugVector.push_back(new DebugCircle(finderPatternCenter,fpRadius,Colors::Red,2));
delete newPattern;
}
}
else
{
//LOGV(LOGTAG_QR,"Size check failed");
//Size check failed
if (FP_DEBUG_ENABLED && debugLevel > 1)
debugVector.push_back(new DebugCircle(Point2i(tempXCenter,tempYCenter),13, Colors::HotPink,1));
}
}
else
{
//LOGV(LOGTAG_QR,"Horizontal check failed");
//Vertical check succeeded, but horizontal re-check failed
if (FP_DEBUG_ENABLED && debugLevel > 1)
debugVector.push_back(new DebugCircle(Point2i(tempXCenter,tempYCenter),12, Colors::OrangeRed,1));
}
}
else
{
//LOGV(LOGTAG_QR,"Variance test failed. AllowedVariance = %d, yCenters = %d,%d,%d [avg=%d], AvgYSize=%d",allowedVariance,tempYCenterArray[0],tempYCenterArray[1],tempYCenterArray[2],tempYCenter,avgYSize);
//Vertical variance test failed
if (FP_DEBUG_ENABLED && debugLevel > 1)
debugVector.push_back(new DebugCircle(Point2i(tempXCenter,tempYCenter),14, Colors::MediumSpringGreen,1));
}
} else
{
//Ratios were correct but vertical check failed
if (FP_DEBUG_ENABLED && debugLevel > 2)
{
if (tempYCenter == 0) //ratio fail
debugVector.push_back(new DebugCircle(Point2i(tempXCenter,y),10,Colors::Aqua,1));
else if (tempYCenter == -1) //topcheck fail
debugVector.push_back(new DebugCircle(Point2i(tempXCenter,y),10,Colors::Orange,1));
else if (tempYCenter == -2) //bottomcheck fail
debugVector.push_back(new DebugCircle(Point2i(tempXCenter,y),10,Colors::Lime,1));
}
}
}
// estimate covariance matrix
void FastLSPredictionComputer::estimate(const Point3i& currentPos) {
if(context->getNeighborhood().getMask().total() != context->getFullNeighborhood().getMask().total())
LSPredictionComputer::estimate(currentPos); // if full neighborhood not yet available at border regions, for simplicity use WLS implementation
else {
Mat sampleVector;
context->contextOf(currentPos, sampleVector); // get current neighborhood and store it in sampleVector
covMat->create(context->getFullNeighborhood().getNumberOfElements(), context->getFullNeighborhood().getNumberOfElements() + 1, CV_64F); // one more row for later variance estimation!
sampleVector.reshape(0, sampleVector.cols).copyTo(covMat->col(covMat->cols - 1)); // put neighborhood in last column for variance estimate
int left = context->getTrainingregion().getLeft(), right = context->getTrainingregion().getRight(), top = context->getTrainingregion().getTop();
(*covMat)(Rect(0, 0, sampleVector.cols, sampleVector.cols))
= getBuffer(currentPos + Point3i( -1, 0, 0))
- getBuffer(currentPos + Point3i( -1, -1, 0))
+ getBuffer(currentPos + Point3i( right, -1, 0))
- getBuffer(currentPos + Point3i(-1-left, 0, 0))
- getBuffer(currentPos + Point3i( right, -1-top, 0))
+ getBuffer(currentPos + Point3i(-1-left, -1-top, 0));
if(context->getTrainingregion().getFront()) { // 3-D training region
int bottom = context->getTrainingregion().getBottom(), front = context->getTrainingregion().getFront();
(*covMat)(Rect(0, 0, sampleVector.cols, sampleVector.cols)) +=
- getBuffer(currentPos + Point3i( -1, 0, -1 ))
+ getBuffer(currentPos + Point3i( -1, -1, -1 ))
- getBuffer(currentPos + Point3i( right, -1, -1 ))
+ getBuffer(currentPos + Point3i(-1-left, 0, -1 ))
+ getBuffer(currentPos + Point3i( right, bottom, -1 ))
- getBuffer(currentPos + Point3i(-1-left, bottom, -1 ))
- getBuffer(currentPos + Point3i( right, bottom, -1-front))
+ getBuffer(currentPos + Point3i(-1-left, bottom, -1-front))
+ getBuffer(currentPos + Point3i( right, -1-top, -1-front))
- getBuffer(currentPos + Point3i(-1-left, -1-top, -1-front));
}
*covMat = covMat->rowRange(0, covMat->rows - 1); // make last row invisible for computePrediction function of WLS
// context->getContextElementsOf(currentPos); // only necessary if computeVariance method from parent class WLS is used
}
*weights = weights->colRange(0, 0); // set used region
} // end FastLSPredictionComputer::estimate
void PushbroomStereo::RunStereoPushbroomStereo2(Mat leftImage, Mat rightImage,Mat laplacian_left,Mat laplacian_right, std::vector<Point3f> *pointVector3d,std::vector<Point3i> *pointVector2d,std::vector<uchar> *pointColors)
{
int row_start = 0;//statet->row_start;
int row_end = leftImage.rows;//statet->row_end;
//PushbroomStereoState state = statet->state;
// we will do this by looping through every block in the left image
// (defined by blockSize) and checking for a matching value on
// the right image
std::vector<Point3f> localHitPoints;
//待确认
int startJ = 0;
int stopJ = leftImage.cols - (m_iDisparity + m_iBlockSize);
if (m_iDisparity < 0)
{
startJ = -m_iDisparity;
stopJ = leftImage.cols - m_iBlockSize;
}
//printf("row_start: %d, row_end: %d, startJ: %d, stopJ: %d, rows: %d, cols: %d\n", row_start, row_end, startJ, stopJ, leftImage.rows, leftImage.cols);
int hitCounter = 0;
//if (state.random_results < 0)
//{
for (int i=row_start; i < row_end;i+=m_iBlockSize)
{
for (int j=startJ; j < stopJ; j+=m_iBlockSize)
{
// get the sum of absolute differences for this location on both images
int sad = GetSAD(leftImage, rightImage, laplacian_left, laplacian_right, j, i);
// check to see if the SAD is below the threshold,
// indicating a hit
if (sad < m_iSadThreshold && sad >= 0)
{
// got a hit
// now check for horizontal invariance (ie check for parts of the image that look the same as this which would indicate that this might be a false-positive)
if (!m_bCheck_horizontal_invariance || (CheckHorizontalInvariance(leftImage, rightImage, laplacian_left, laplacian_right, j, i)== false))
{
// add it to the vector of matches
// don't forget to offset it by the blockSize,so we match the center of the block instead of the top left corner
localHitPoints.push_back(Point3f(j+m_iBlockSize/2.0, i+m_iBlockSize/2.0, -m_iDisparity));
//localHitPoints.push_back(Point3f(state.debugJ, state.debugI, -disparity));
uchar pxL = leftImage.at<uchar>(i,j);
pointColors->push_back(pxL); // this is the corner of the box, not the center
hitCounter ++;
if (m_bShow_display)
pointVector2d->push_back(Point3i(j, i, sad));
} // check horizontal invariance
}
}
}
// now we have an array of hits -- transform them to 3d points
if (hitCounter > 0)
perspectiveTransform(localHitPoints, *pointVector3d, m_matQ);
}
Point3i Point3i::interpolate_to(const float &rhs_part, const Point3i &rhs) const {
const float lhs_part = 1.0f - rhs_part;
return Point3i(int(lhs_part * x + rhs_part * rhs.x),
int(lhs_part * y + rhs_part * rhs.y),
int(lhs_part * z + rhs_part * rhs.z));
}
Alien &Alien::animate(double secPerFrame){
Point3i rel = Point3i();
Point3f alienspeed = Point3f();
float M;
float R;
if(active){
// If the alien flies off the screen, it dies
if(place.z > ZERO){
/* die();
Vaus * v = game->getVaus();
R = baseRad + v->rad;
Point3f distance(place.x - v->place.x,
place.y - v->place.y,
ZERO);
M = distance.res3f();//distance resultant
if(M < R){
game->killVaus();
vaus->active = false;
for(i = 0; i < VAUS_PARTICLES; i++){
vaus->explosion[i].active = TRUE;
vaus->explosion[i].explode(vaus->place, vaus->pal, VAUS_COLORS, vaus->particle_rad);
}
}*/
}else{
//FIXME: rel = Functions::coords(&place);
int i = 0;
Ball * ball = game->getActiveBall();
if(ball){
float ballspeed = ball->speed.res3f();
alienspeed = place.chase(ball->place,ballspeed);
}
speed = speed.deepcopy(alienspeed);
Brick * brick = game->getBrickAt(place);
if(brick){
speed.x = -speed.x;
speed.y = -speed.y;
speed.z = -speed.z;
place.z -= size.z;
}
if(place.x >= SCENE_MAX - size.x/2){
// a safer but more verbose way to confine aliens within the game scene.
if(speed.x > ZERO){
speed.x = -speed.x;
place.x += speed.x * size.x/abs(speed.x);
}
}
if(place.x <= SCENE_MIN + size.x/2){
if(speed.x < ZERO){
speed.x = -speed.x;
place.x += speed.x * size.x/abs(speed.x);
}
}
if(place.y >= SCENE_MAX - size.y/2){
if(speed.y > ZERO){
speed.y = -speed.y;
place.y += speed.y * size.y/abs(speed.y);
}
}
if(place.y <= SCENE_MIN + size.y/2){
if(speed.y < ZERO){
speed.y = -speed.y;
place.y += speed.y * size.y/abs(speed.y);
}
}
if(place.z <= SCENE_MIN - SCENE_MAX + size.z/2){
if(speed.z < ZERO){
speed.z = - speed.z;
place.z += speed.z * size.z/abs(speed.z);
}
}
place.x += speed.x * secPerFrame;
place.y += speed.y * secPerFrame;
place.z += zSpeed * secPerFrame;
roty += rotSpeed * secPerFrame;
}
}
return *this;
}
