Smoothing Small Data Set With Second Order Quadratic Curve - c++

I'm doing some specific signal analysis, and I am in need of a method that would smooth out a given bell-shaped distribution curve. A running average approach isn't producing the results I desire. I want to keep the min/max, and general shape of my fitted curve intact, but resolve the inconsistencies in sampling.
In short: if given a set of data that models a simple quadratic curve, what statistical smoothing method would you recommend?
If possible, please reference an implementation, library, or framework.
Thanks SO!
Edit: Some helpful data
(A possible signal graph)
The dark colored quadratic is my "fitted" curve of the light colored connected data points.
The sample # -44 (approx.), is a problem in my graph (i.e. a potential sample inconsistency). I need this curve to "fit" the distribution better, and overcome the values that do not trend accordingly. Hope this helps!

A "quadratic" curve is one thing; "bell-shaped" usually means a Gaussian normal distribution. Getting a best-estimate Gaussian couldn't be easier: you compute the sample mean and variance and your smooth approximation is
y = exp(-squared(x-mean)/variance)
If, on the other hand, you want to approximate a smooth curve with a quadradatic, I'd recommend computing a quadratic polynomial with minimum square error. I can nenver remember the formulas for this, but if you've had differential calculus, write the formula for the total square error (pointwise) and differentiate with respect to the coefficients of your quadratic. Set the first derivatives to zero and solve for the best approximation. Or you could look it up.
Finally, if you just want a smooth-looking curve to approximate a set of points, cubic splines are your best bet. The curves won't necessarily mean anything, but you'll get a nice smooth approximation.

#include <iostream>
#include <math.h>
struct WeightedData
{
double x;
double y;
double weight;
};
void findQuadraticFactors(WeightedData *data, double &a, double &b, double &c, unsigned int const datasize)
{
double w1 = 0.0;
double wx = 0.0, wx2 = 0.0, wx3 = 0.0, wx4 = 0.0;
double wy = 0.0, wyx = 0.0, wyx2 = 0.0;
double tmpx, tmpy;
double den;
for (unsigned int i = 0; i < datasize; ++i)
{
double x = data[i].x;
double y = data[i].y;
double w = data[i].weight;
w1 += w;
tmpx = w * x;
wx += tmpx;
tmpx *= x;
wx2 += tmpx;
tmpx *= x;
wx3 += tmpx;
tmpx *= x;
wx4 += tmpx;
tmpy = w * y;
wy += tmpy;
tmpy *= x;
wyx += tmpy;
tmpy *= x;
wyx2 += tmpy;
}
den = wx2 * wx2 * wx2 - 2.0 * wx3 * wx2 * wx + wx4 * wx * wx + wx3 * wx3 * w1 - wx4 * wx2 * w1;
if (den == 0.0)
{
a = 0.0;
b = 0.0;
c = 0.0;
}
else
{
a = (wx * wx * wyx2 - wx2 * w1 * wyx2 - wx2 * wx * wyx + wx3 * w1 * wyx + wx2 * wx2 * wy - wx3 * wx * wy) / den;
b = (-wx2 * wx * wyx2 + wx3 * w1 * wyx2 + wx2 * wx2 * wyx - wx4 * w1 * wyx - wx3 * wx2 * wy + wx4 * wx * wy) / den;
c = (wx2 * wx2 * wyx2 - wx3 * wx * wyx2 - wx3 * wx2 * wyx + wx4 * wx * wyx + wx3 * wx3 * wy - wx4 * wx2 * wy) / den;
}
}
double findY(double const a, double const b, double const c, double const x)
{
return a * x * x + b * x + c;
};
int main(int argc, char* argv[])
{
WeightedData data[9];
data[0].weight=1; data[0].x=1; data[0].y=-52.0;
data[1].weight=1; data[1].x=2; data[1].y=-48.0;
data[2].weight=1; data[2].x=3; data[2].y=-43.0;
data[3].weight=1; data[3].x=4; data[3].y=-44.0;
data[4].weight=1; data[4].x=5; data[4].y=-35.0;
data[5].weight=1; data[5].x=6; data[5].y=-31.0;
data[6].weight=1; data[6].x=7; data[6].y=-32.0;
data[7].weight=1; data[7].x=8; data[7].y=-43.0;
data[8].weight=1; data[8].x=9; data[8].y=-52.0;
double a=0.0, b=0.0, c=0.0;
findQuadraticFactors(data, a, b, c, 9);
std::cout << " x \t y" << std::endl;
for (int i=0; i<9; ++i)
{
std::cout << " " << data[i].x << ", " << findY(a,b,c,data[i].x) << std::endl;
}
}

How about a simple digital low-pass filter?
y[0] = x[0];
for (i = 1; i < len; ++i)
y[i] = a * x[i] + (1.0 - a) * y[i - 1];
In this case, x[] is your input data and y[] is the filtered output. The a coefficient is a value between 0 and 1 that you should tweak. An a value of 1 reproduces the input and the cut-off frequency decreases as a approaches 0.

Perhaps the parameters for your running average are set wrong (sample window too small or large)?
Is it just noise superimposed on your bell curve? How close is the noise frequency to that of the signal you're trying to retrieve? A picture of what you're trying to extract might help us identify a solution.
You could try some sort of fitting algorithm using a least squares fit if you can make a reasonable guess of the function parameters. Those sorts of techniques often have some immunity to noise.

Related

Interpreting visual studio profiler, is this subtraction slow? Can I make all this any faster?

I'm using the Visual Studio profiler for the first time and I'm trying to interpret the results. Looking at the percentages on the left, I found this subtraction's time cost a bit strange:
Other parts of the code contain more complex expressions, like:
Even a simple multiplication seems way faster than the subtraction :
Other multiplications take way longer and I really don't get why, like this :
So, I guess my question is if there is anything weird going on here.
Complex expressions take longer than that subtraction and some expressions take way longer than similar other ones. I run the profiler several times and the distribution of the percentages is always like this. Am I just interpreting this wrong?
Update:
I was asked to give the profile for the whole function so here it is, even though it's a bit big. I ran the function inside a for loop for 1 minute and got 50k samples. The function contains a double loop. I include the text first for ease, followed by the pictures of profiling. Note that the code in text is a bit updated.
for (int i = 0; i < NUMBER_OF_CONTOUR_POINTS; i++) {
vec4 contourPointV(contour3DPoints[i], 1);
float phi = angles[i];
float xW = pose[0][0] * contourPointV.x + pose[1][0] * contourPointV.y + contourPointV.z * pose[2][0] + pose[3][0];
float yW = pose[0][1] * contourPointV.x + pose[1][1] * contourPointV.y + contourPointV.z * pose[2][1] + pose[3][1];
float zW = pose[0][2] * contourPointV.x + pose[1][2] * contourPointV.y + contourPointV.z * pose[2][2] + pose[3][2];
float x = -G_FU_STRICT * xW / zW;
float y = -G_FV_STRICT * yW / zW;
x = (x + 1) * G_WIDTHo2;
y = (y + 1) * G_HEIGHTo2;
y = G_HEIGHT - y;
phi -= extraTheta;
if (phi < 0)phi += CV_PI2;
int indexForTable = phi * oneKoverPI;
//vec2 ray(cos(phi), sin(phi));
vec2 ray(cos_pre[indexForTable], sin_pre[indexForTable]);
vec2 ray2(-ray.x, -ray.y);
float outerStepX = ray.x * step;
float outerStepY = ray.y * step;
cv::Point2f outerPoint(x + outerStepX, y + outerStepY);
cv::Point2f innerPoint(x - outerStepX, y - outerStepY);
cv::Point2f contourPointCV(x, y);
cv::Point2f contourPointCVcopy(x, y);
bool cut = false;
if (!isInView(outerPoint.x, outerPoint.y) || !isInView(innerPoint.x, innerPoint.y)) {
cut = true;
}
bool outside2 = true; bool outside1 = true;
if (cut) {
outside2 = myClipLine(contourPointCV.x, contourPointCV.y, outerPoint.x, outerPoint.y, G_WIDTH - 1, G_HEIGHT - 1);
outside1 = myClipLine(contourPointCVcopy.x, contourPointCVcopy.y, innerPoint.x, innerPoint.y, G_WIDTH - 1, G_HEIGHT - 1);
}
myIterator innerRayMine(contourPointCVcopy, innerPoint);
myIterator outerRayMine(contourPointCV, outerPoint);
if (!outside1) {
innerRayMine.end = true;
innerRayMine.prob = true;
}
if (!outside2) {
outerRayMine.end = true;
innerRayMine.prob = true;
}
vec2 normal = -ray;
float dfdxTerm = -normal.x;
float dfdyTerm = normal.y;
vec3 point3D = vec3(xW, yW, zW);
cv::Point contourPoint((int)x, (int)y);
float Xc = point3D.x; float Xc2 = Xc * Xc; float Yc = point3D.y; float Yc2 = Yc * Yc; float Zc = point3D.z; float Zc2 = Zc * Zc;
float XcYc = Xc * Yc; float dfdxFu = dfdxTerm * G_FU; float dfdyFv = dfdyTerm * G_FU; float overZc2 = 1 / Zc2; float overZc = 1 / Zc;
pixelJacobi[0] = (dfdyFv * (Yc2 + Zc2) + dfdxFu * XcYc) * overZc2;
pixelJacobi[1] = (-dfdxFu * (Xc2 + Zc2) - dfdyFv * XcYc) * overZc2;
pixelJacobi[2] = (-dfdyFv * Xc + dfdxFu * Yc) * overZc;
pixelJacobi[3] = -dfdxFu * overZc;
pixelJacobi[4] = -dfdyFv * overZc;
pixelJacobi[5] = (dfdyFv * Yc + dfdxFu * Xc) * overZc2;
float commonFirstTermsSum = 0;
float commonFirstTermsSquaredSum = 0;
int test = 0;
while (!innerRayMine.end) {
test++;
cv::Point xy = innerRayMine.pos(); innerRayMine++;
int x = xy.x;
int y = xy.y;
float dx = x - contourPoint.x;
float dy = y - contourPoint.y;
vec2 dxdy(dx, dy);
float raw = -glm::dot(dxdy, normal);
float heavisideTerm = heaviside_pre[(int)raw * 100 + 1000];
float deltaTerm = delta_pre[(int)raw * 100 + 1000];
const Vec3b rgb = ante[y * 640 + x];
int red = rgb[0]; int green = rgb[1]; int blue = rgb[2];
red = red >> 3; red = red << 10; green = green >> 3; green = green << 5; blue = blue >> 3;
int colorIndex = red + green + blue;
pF = pFPointer[colorIndex];
pB = pBPointer[colorIndex];
float denAsMul = 1 / (pF + pB + 0.000001);
pF = pF * denAsMul;
float pfMinusPb = 2 * pF - 1;
float denominator = heavisideTerm * (pfMinusPb)+pB + 0.000001;
float commonFirstTerm = -pfMinusPb / denominator * deltaTerm;
commonFirstTermsSum += commonFirstTerm;
commonFirstTermsSquaredSum += commonFirstTerm * commonFirstTerm;
}
}
Visual Studio profiles by sampling: it interrupts execution often and records the value of the instruction pointer; it then maps it to the source and calculates the frequency of hitting that line.
There are few issues with that: it's not always possible to figure out which line produced a specific assembly instruction in the optimized code.
One trick I use is to move the code of interest into a separate function and declare it with __declspec(noinline) .
In your example, are you sure the subtraction was performed as many times as multiplication? I would be more puzzled by the difference in subsequent multiplication (0.39% and 0.53%)
Update:
I believe that the following lines:
float phi = angles[i];
and
phi -= extraTheta;
got moved together in assembly and the time spent getting angles[i] was added to that subtraction line.

Why isn't my 4 thread implementation faster than the single thread one?

I don't know much about multi-threading and I have no idea why this is happening so I'll just get to the point.
I'm processing an image and divide the image in 4 parts and pass each part to each thread(essentially I pass the indices of the first and last pixel rows of each part). For example, if the image has 1000 rows, each thread will process 250 of them. I can go in details about my implementation and what I'm trying to achieve in case it can help you. For now I provide the code executed by the threads in case you can detect why this is happening. I don't know if it's relevant but in both cases(1 thread or 4 threads) the process takes around 15ms and pfUMap and pbUMap are unordered maps.
void jacobiansThread(int start, int end,vector<float> &sJT,vector<float> &sJTJ) {
uchar* rgbPointer;
float* depthPointer;
float* sdfPointer;
float* dfdxPointer; float* dfdyPointer;
float fov = radians(45.0);
float aspect = 4.0 / 3.0;
float focal = 1 / (glm::tan(fov / 2));
float fu = focal * cols / 2 / aspect;
float fv = focal * rows / 2;
float strictFu = focal / aspect;
float strictFv = focal;
vector<float> pixelJacobi(6, 0);
for (int y = start; y <end; y++) {
rgbPointer = sceneImage.ptr<uchar>(y);
depthPointer = depthBuffer.ptr<float>(y);
dfdxPointer = dfdx.ptr<float>(y);
dfdyPointer = dfdy.ptr<float>(y);
sdfPointer = sdf.ptr<float>(y);
for (int x = roiX.x; x <roiX.y; x++) {
float deltaTerm;// = deltaPointer[x];
float raw = sdfPointer[x];
if (raw > 8.0)continue;
float dirac = (1.0f / float(CV_PI)) * (1.2f / (raw * 1.44f * raw + 1.0f));
deltaTerm = dirac;
vec3 rgb(rgbPointer[x * 3], rgbPointer[x * 3+1], rgbPointer[x * 3+2]);
vec3 bin = rgbToBin(rgb, numberOfBins);
int indexOfColor = bin.x * numberOfBins * numberOfBins + bin.y * numberOfBins + bin.z;
float s3 = glfwGetTime();
float pF = pfUMap[indexOfColor];
float pB = pbUMap[indexOfColor];
float heavisideTerm;
heavisideTerm = HEAVISIDE(raw);
float denominator = (heavisideTerm * pF + (1 - heavisideTerm) * pB) + 0.000001;
float commonFirstTerm = -(pF - pB) / denominator * deltaTerm;
if (pF == pB)continue;
vec3 pixel(x, y, depthPointer[x]);
float dfdxTerm = dfdxPointer[x];
float dfdyTerm = -dfdyPointer[x];
if (pixel.z == 1) {
cv::Point c = findClosestContourPoint(cv::Point(x, y), dfdxTerm, -dfdyTerm, abs(raw));
if (c.x == -1)continue;
pixel = vec3(c.x, c.y, depthBuffer.at<float>(cv::Point(c.x, c.y)));
}
vec3 point3D = pixel;
pixelToViewFast(point3D, cols, rows, strictFu, strictFv);
float Xc = point3D.x; float Xc2 = Xc * Xc; float Yc = point3D.y; float Yc2 = Yc * Yc; float Zc = point3D.z; float Zc2 = Zc * Zc;
pixelJacobi[0] = dfdyTerm * ((fv * Yc2) / Zc2 + fv) + (dfdxTerm * fu * Xc * Yc) / Zc2;
pixelJacobi[1] = -dfdxTerm * ((fu * Xc2) / Zc2 + fu) - (dfdyTerm * fv * Xc * Yc) / Zc2;
pixelJacobi[2] = -(dfdyTerm * fv * Xc) / Zc + (dfdxTerm * fu * Yc) / Zc;
pixelJacobi[3] = -(dfdxTerm * fu) / Zc;
pixelJacobi[4] = -(dfdyTerm * fv) / Zc;
pixelJacobi[5] = (dfdyTerm * fv * Yc) / Zc2 + (dfdxTerm * fu * Xc) / Zc2;
float weightingTerm = -1.0 / log(denominator);
for (int i = 0; i < 6; i++) {
pixelJacobi[i] *= commonFirstTerm;
sJT[i] += pixelJacobi[i];
}
for (int i = 0; i < 6; i++) {
for (int j = i; j < 6; j++) {
sJTJ[i * 6 + j] += weightingTerm * pixelJacobi[i] * pixelJacobi[j];
}
}
}
}
}
This is the part where I call each thread:
vector<std::thread> myThreads;
float step = (roiY.y - roiY.x) / numberOfThreads;
vector<vector<float>> tsJT(numberOfThreads, vector<float>(6, 0));
vector<vector<float>> tsJTJ(numberOfThreads, vector<float>(36, 0));
for (int i = 0; i < numberOfThreads; i++) {
int start = roiY.x+i * step;
int end = start + step;
if (end > roiY.y)end = roiY.y;
myThreads.push_back(std::thread(&pwp3dV2::jacobiansThread, this,start,end,std::ref(tsJT[i]), std::ref(tsJTJ[i])));
}
vector<float> sJT(6, 0);
vector<float> sJTJ(36, 0);
for (int i = 0; i < numberOfThreads; i++)myThreads[i].join();
Other Notes
To measure time I used glfwGetTime() before and right after the second code snippet. The measurements vary but the average is about 15ms as I mentioned, for both implementations.
Starting a thread has significant overhead, which might not be worth the time if you have only 15 milliseconds worth of work.
The common solution is to keep threads running in the background and send them data when you need them, instead of calling the std::thread constructor to create a new thread every time you have some work to do.
Pure spectaculation but two things might be preventing the full power of parallelization.
Processing speed is limited by the memory bus. Cores will wait until data is loaded before continuing.
Data sharing between cores. Some caches are core specific. If memory is shared between cores, data must traverse down to shared cache before loading.
On Linux you can use Perf to check for cache misses.
if you wanna better time you need to split a cycle runs from a counter, for this you need to do some preprocessing. some fast stuff like make an array of structures with headers for each segment or so. if say you can't mind anything better you can just do vector<int> with values of a counter. Then do for_each(std::execution::par,...) on that. way much faster.
for timings there's
auto t2 = std::chrono::system_clock::now();
std::chrono::milliseconds f = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);

Function to calculate angle to a point in unusual 2D space

I'm looking for a robust function to calculate the difference(delta) between an object and a point.
For example, it there was an object at point A with an orientation of 1.2 Rad, what would be the required angle for the object to turn in order to face Point B.
Furthermore, I'm working in a odd coordinate system where north(0 Rad) faces towards +X, the image below shows this.
I understand the fundamentals, but I'm struggling to produce something robust.
My c++ function template look like this,
float Robot::getDeltaHeading(float _x1, float _y1, float _x2, float _y2, float _currentHeading) {
//TODO:
return xxxxxxx;
}
Any help would be appreciated.
Cheers in Advance.
Here's the answer.
float Robot::getDeltaHeading(float _x1, float _y1, float _x2, float _y2, float _currentHeading) {
_currentHeading -= 90;
double Ux = 0.0, Uy = 0.0, Vx = 0.0, Vy = 0.0, d = 0.0;
d = sqrtf(powf(abs(_x1 - _x2), 2) + powf(abs(_y1 - _x2), 2));
Ux = (_x2 - _x1) / d;
Uy = (_y2 - _y1) / d;
Vx = cos(_currentHeading * (3.14159f / 180.0));
Vy = sin(_currentHeading * (3.14159f / 180.0));
auto ans = 90 + (atan2(((Ux * Vy) - (Uy * Vx)), ((Ux * Vx) + (Uy * Vy))) * (180.0 / 3.14159f));
while (ans > 180) ans -= 360;
while (ans < -180) ans += 360;
return ans;
}

Why are my openGL ellipses pointed?

I copied this ellipse code directly from the opengl textbook:
void ellipseMidpoint (int xCenter, int yCenter, int Rx, int Ry)
{
int Rx2 = Rx * Rx;
int Ry2 = Ry * Ry;
int twoRx2 = 2 * Rx2;
int twoRy2 = 2 * Ry2;
int p;
int x = 0;
int y = Ry;
int px = 0;
int py = twoRx2 * y;
//initial points in both quadrants
ellipsePlotPoints (xCenter, yCenter, x, y);
//Region 1
p = round (Ry2 - (Rx2 * Ry) + (0.25 * Rx2));
while (px < py) {
x++;
px += twoRy2;
if (p < 0)
p += Ry2 + px;
else {
y--;
py -= twoRx2;
p += Ry2 + px - py;
}
ellipsePlotPoints (xCenter, yCenter, x, y);
}
//Region 2
p = round (Ry2 * (x+0.5) * (x+0.5) + Rx2 * (y-1) * (y-1) - Rx2 * Ry2);
while (y > 0) {
y--;
py -= twoRx2;
if (p > 0)
p += Rx2 - py;
else {
x++;
px += twoRy2;
p += Rx2 - py + px;
}
ellipsePlotPoints (xCenter, yCenter, x, y);
}
}
void ellipsePlotPoints (int xCenter, int yCenter, int x, int y)
{
setPixel (xCenter + x, yCenter + y);
setPixel (xCenter - x, yCenter + y);
setPixel (xCenter + x, yCenter - y);
setPixel (xCenter - x, yCenter - y);
}
void setPixel (GLint xPos, GLint yPos)
{
glBegin (GL_POINTS);
glVertex2i(xPos, yPos);
glEnd();
}
The smaller ellipses seem to be fine but the larger ones are pointy and sort of flat at the ends.
Any ideas why?
Here is a current screenshot:
I think you're encountering overflow. I played with your code. While I never saw exactly the same "lemon" type shapes from your pictures, things definitely fell apart at large sizes, and it was caused by overflowing the range of the int variables used in the code.
For example, look at one of the first assignments:
int py = twoRx2 * y;
If you substitute, this becomes:
int py = 2 * Rx * Rx * Ry;
If you use a value of 1000 each for Rx and Ry, this is 2,000,000,000. Which is very close to the 2^31 - 1 top of the range of a 32-bit int.
If you want to use this algorithm for larger sizes, you could use 64-bit integer variables. Depending on your system, the type would be long or long long. Or more robustly, int64_t after including <stdint.h>.
Now, if all you want to do is draw an ellipsis with OpenGL, there are much better ways. The Bresenham type algorithms used in your code are ideal if you need to draw a curve pixel by pixel. But OpenGL is a higher level API, which knows how to render more complex primitives than just pixels. For a curve, you will most typically use a connected set of line segments to approximate the curve. OpenGL will then take care of turning those line segments into pixels.
The simplest way to draw an ellipsis is to directly apply the parametric representation. With phi an angle between 0 and PI, and using the naming from your code, the points on the ellipsis are:
x = xCenter + Rx * cos(phi)
y = yCenter + Ry * sin(phi)
You can use an increment for phi that meets your precision requirements, and the code will look something to generate an ellipsis approximated by DIV_COUNT points will look something like this:
float angInc = 2.0f * m_PI / (float)DIV_COUNT;
float ang = 0.0f;
glBegin(GL_LINE_LOOP);
for (int iDiv = 0; iDiv < DIV_COUNT; ++iDiv) {
ang += angInc;
float x = xCenter + Rx * cos(ang);
float y = yCenter + Ry * sin(ang);
glVertex2f(x, y);
glEnd();
If you care about efficiency, you can avoid calculating the trigonometric functions for each point, and apply an incremental rotation to calculate each point from the previous one:
float angInc = 2.0f * M_PI / (float)DIV_COUNT;
float cosInc = cos(angInc);
float sinInc = sin(angInc);
float cosAng = 1.0f;
float sinAng = 0.0f
glBegin(GL_LINE_LOOP);
for (int iDiv = 0; iDiv < DIV_COUNT; ++iDiv) {
float newCosAng = cosInc * cosAng - sinInc * sinAng;
sinAng = sinInc * cosAng + cosInc * sinAng;
cosAng = newCosAng;
float x = xCenter + Rx * cosAng;
float y = yCenter + Ry * sinAng;
glVertex2f(x, y);
glEnd();
This code is of course just for illustrating the math, and to get you started. In reality, you should use current OpenGL rendering methods, which includes vertex buffers, etc.

What's wrong with my BRDF programm in CG

Now I'm working on a basic CG program about the BRDF. And after I got the image, it seems that, all the points face to the light is too bright, I don't know the reason. And here's my code, where I tried to invoke the lookup_brdf_val function.
Vec3f hitNormal = ray.hit->getNormal(ray);
if(hitNormal * ray.dir > 0)
hitNormal = -hitNormal;
result = Vec3f(0, 0, 0);
Ray lightRay;
lightRay.org = ray.org + ray.dir * ray.t;
Vec3f intensity;
for(unsigned int l = 0; l < scene->lights.size(); l++)
{
scene->lights[l]->illuminate(lightRay, intensity);
if(!scene->isOccluded(lightRay))
{
double theta1,theta2;
// Calculate the theta1 and theta2.
theta1 = acosf(-(ray.dir * hitNormal));
theta2 = acosf(lightRay.dir * hitNormal);
// Calculate the fi1 and fi2.
double fi1 = 0;
Vec3f O = ray.org + ray.dir * ray.t;
Vec3f A = O - ray.dir;
Vec3f C = (ray.dir * hitNormal) * hitNormal + A;
Vec3f B = lightRay.dir + O;
Vec3f D = ((-lightRay.dir) * hitNormal) * hitNormal + B;
Vec3f OC = C - O;
Vec3f OD = D - O;
double fi2 = acosf((OD * OC) / (length(OD) * length(OC)));
double x = 0;
double y = 0;
double z = 0;
double &r = x;
double &g = y;
double &b = z;
read->lookup_brdf_val(theta1, fi1, theta2, fi2, r, g, b);
result += Vec3f(r * scale.x * intensity.x, g * scale.y * intensity.y, b * scale.z * intensity.z);
I suggest start from a simpler BRDF to make sure that your main loop is not broken -- try something simple like lambert: max(0,dot(lightRay,hitNormal)) and be sure that those are normalized vectors. Divide by scene->lights.size() if it's simply too bright because you have too many lights.
If the image looks correct with a simple BRDF, now just try it with variations of your other components. You don't give the code for lookup_brdf_val() at all, so beyond that one can only speculate.
It's just like any other programming, though. Reduce the # of variables until you find the one that's awry.