int main(int argc, char* argv[]) {
store_map();
init_position(0);
display();
printf("prediction\n");
prediction(1);// we move from one meter to the right
display();
printf("estimation\n");
estimation(wall,wall);
normalization();
display();
printf("prediction\n");
prediction(1);// we move from one meter to the right
display();
printf("estimation\n");
estimation(wall,wall);
normalization();
display();
printf("prediction\n");
prediction(1);// we move from one meter to the right
display();
printf("estimation\n");
estimation(wall,wall);
normalization();
display();
printf("prediction\n");
prediction(1);// we move from one meter to the right
display();
printf("estimation\n");
estimation(wall,wall);
normalization();
display();
printf("prediction\n");
prediction(1);// we move from one meter to the right
display();
printf("estimation\n");
estimation(wall,wall);
normalization();
display();
return 0;
}// main
void
pcl::people::HOG::compute (float *I, float *descriptor) const
{
// HOG computation:
float *M, *O, *G, *H;
M = new float[h_ * w_];
O = new float[h_ * w_];
H = new float[(w_ / bin_size_) * (h_ / bin_size_) * n_orients_]();
G = new float[(w_ / bin_size_) * (h_ / bin_size_) * n_orients_ *4]();
// Compute gradient magnitude and orientation at each location (uses sse):
gradMag (I, h_, w_, n_channels_, M, O );
// Compute n_orients gradient histograms per bin_size x bin_size block of pixels:
gradHist ( M, O, h_, w_, bin_size_, n_orients_, soft_bin_, H );
// Apply normalizations:
normalization ( H, h_, w_, bin_size_, n_orients_, clip_, G );
// Select descriptor of internal part of the image (remove borders):
int k = 0;
for (int l = 0; l < (n_orients_ * 4); l++)
{
for (int j = 1; j < (w_ / bin_size_ - 1); j++)
{
for (int i = 1; i < (h_ / bin_size_ - 1); i++)
{
descriptor[k] = G[i + j * h_ / bin_size_ + l * (h_ / bin_size_) * (w_ / bin_size_)];
k++;
}
}
}
delete[] M; delete[] O; delete[] H; delete[] G;
}
Float SHVector::eval(Float theta, Float phi) const {
Float result = 0;
Float cosTheta = std::cos(theta);
Float *sinPhi = (Float *) alloca(sizeof(Float)*m_bands),
*cosPhi = (Float *) alloca(sizeof(Float)*m_bands);
for (int m=0; m<m_bands; ++m) {
sinPhi[m] = std::sin((m+1) * phi);
cosPhi[m] = std::cos((m+1) * phi);
}
for (int l=0; l<m_bands; ++l) {
for (int m=1; m<=l; ++m) {
Float L = legendreP(l, m, cosTheta) * normalization(l, m);
result += operator()(l, -m) * SQRT_TWO * sinPhi[m-1] * L;
result += operator()(l, m) * SQRT_TWO * cosPhi[m-1] * L;
}
result += operator()(l, 0) * legendreP(l, 0, cosTheta) * normalization(l, 0);
}
return result;
}
Float SHVector::eval(const Vector &v) const {
Float result = 0;
Float cosTheta = v.z, phi = std::atan2(v.y, v.x);
if (phi < 0) phi += 2*M_PI;
Float *sinPhi = (Float *) alloca(sizeof(Float)*m_bands),
*cosPhi = (Float *) alloca(sizeof(Float)*m_bands);
for (int m=0; m<m_bands; ++m) {
sinPhi[m] = std::sin((m+1) * phi);
cosPhi[m] = std::cos((m+1) * phi);
}
for (int l=0; l<m_bands; ++l) {
for (int m=1; m<=l; ++m) {
Float L = legendreP(l, m, cosTheta) * normalization(l, m);
result += operator()(l, -m) * SQRT_TWO * sinPhi[m-1] * L;
result += operator()(l, m) * SQRT_TWO * cosPhi[m-1] * L;
}
result += operator()(l, 0) * legendreP(l, 0, cosTheta) * normalization(l, 0);
}
return result;
}
void write_installer_conf(const char* path)
{
g_assert(path != NULL);
normalization();
char* content = installer_conf_to_string();
GError* error = NULL;
g_file_set_contents(path, content, -1, &error);
g_free(content);
if (error != NULL) {
g_warning("write deepin-installer.conf(to %s) failed:%s\n", path, error->message);
g_error_free(error);
}
}
void bow_normalization(const IMbdd& bdd, struct svm_problem& svmProblem){
std::string normalization(bdd.getNormalization());
if(normalization.compare("simple") == 0 || normalization.compare("both") == 0)
bow_simple_normalization(svmProblem);
if(normalization.compare("gaussian") == 0|| normalization.compare("both") == 0){
int k = bdd.getK();
double means[k], stand_devia[k];
load_gaussian_parameters(bdd,
means,
stand_devia);
bow_gaussian_normalization(k,
means,
stand_devia,
svmProblem);
}
}
int minimax_threat(int player, luint m[4], int depth, int hash, int u, int v){
//minimax ale player musí hrat na hranu (u,v) ktera je volna
// print_adjacency_matrix(m,"");
timer++;
luint m2[4];
int hash2;
if ( NORMALIZATION_FREQUENCY > 0 && depth % NORMALIZATION_FREQUENCY == 0)
hash = normalization(m,hash);
int winner;
if ( (winner = get_from_cache(m,hash) ) != 42){
// if ( depth % 3 == 1 && (winner = get_from_cache(m,hash) ) != 42 ) {
//je v cachy
return winner;
}
if (depth == (N*(N-1))/2 )
//vse je obarvene remiza
//TODO diky poctu zbylych hran a tomu jake jsou hrozby by mohlo jit skoncit driv
return 0;
int x,y;
int t = threats(player,m,u,v,&x,&y);
if (t > 1){
if (player == GREEN)
return 1;
else
return -1;
}
copy_graph(m2,m);
hash2 = set_edge_color(player,m2,hash,u,v);
if (t == 1){
winner = minimax_threat(next(player),m2,depth+1,hash2,x,y);
} else {
winner = minimax(next(player),m2,depth+1,hash2);
}
if ( depth % 3 == 1 )
put_into_cache(m2,hash2,winner);
return winner;
}
void im_normalize_bdd_bow(const IMbdd& bdd, const std::vector<std::string>& trainingPeople,
std::map<std::string, struct svm_problem>& peopleBOW){
int k = bdd.getK();
// Extract and export gaussian parameters
struct svm_problem svmProblem;
svmProblem.l = 0;
svmProblem.x = NULL;
svmProblem.y = NULL;
for(std::vector<std::string>::const_iterator person = trainingPeople.begin();
person != trainingPeople.end();
++person){
for(int i=0 ; i<peopleBOW[*person].l ; i++){
svm_node *bow = peopleBOW[*person].x[i];
addBOW(bow, peopleBOW[*person].y[i], svmProblem);
}
}
double *means=NULL, *stand_devia=NULL;
means = new double[k];
stand_devia = new double[k];
std::cout << "Extracting gaussian parameters..." << std::endl;
get_gaussian_parameters(k,
svmProblem,
means,
stand_devia);
destroy_svm_problem(svmProblem);
std::cout<<"Exporting gaussian parameters..." << std::endl;
save_gaussian_parameters(bdd,
means,
stand_devia);
delete []means;
delete []stand_devia;
std::vector<std::string> people = bdd.getPeople();
std::string normalization(bdd.getNormalization());
if(normalization.compare("") !=0){
std::cout << "Doing the " << normalization << " normalization..." << std::endl;
for(std::vector<std::string>::iterator person = people.begin();
person != people.end();
++person){
bow_normalization(bdd,peopleBOW[*person]);
}
}
}
int minimax(int player, luint m[4], int depth, int hash){
//1 zeleny vyhraje
//0 remiza obarvene bez k4
//-1 cerveny vyhraje
// print_adjacency_matrix(m,"");
timer++;
int max = -1;
int min = 1;
luint m2[4];
int hash2 = 0;
if ( NORMALIZATION_FREQUENCY > 0 && depth % NORMALIZATION_FREQUENCY == 0)
hash = normalization(m,hash);
int winner;
if ( (winner = get_from_cache(m,hash) ) != 42 && depth > 0){
// if ( depth % 3 == 1 && (winner = get_from_cache(m,hash) ) != 42 ) {
//je v cachy a zaroven neni prazdny
return winner;
}
if ( depth == (N*(N-1))/2 )
//vse je obarvene remiza
return 0;
for (uint i=0; i<N; i++)
for (uint j=i+1; j<N; j++){
if (get_edge_color(m,i,j) == 0){
//pro vsechny neobarvene hrany (i,j)
int x,y;
int t = threats(player,m,i,j,&x,&y);
if (t > 1){
if (player == GREEN)
return 1;
else
return -1;
}
/*
if (win(player,m,i,j)){
if (player == GREEN)
return 1;
else
return -1;
}
*/ copy_graph(m2,m);
hash2 = set_edge_color(player,m2,hash,i,j);
int tmp;
if (t == 1){
tmp = minimax_threat(next(player),m2,depth+1,hash2,x,y);
} else {
tmp = minimax(next(player),m2,depth+1,hash2);
}
if (tmp > max)
max = tmp;
if (tmp < min)
min = tmp;
}
}
if (player == GREEN)
winner = max;
else
winner = min;
if ( depth % 3 == 1 )
put_into_cache(m2,hash2,winner);
return winner;
}
Float SHVector::evalAzimuthallyInvariant(const Vector &v) const {
Float result = 0, cosTheta = v.z;
for (int l=0; l<m_bands; ++l)
result += operator()(l, 0) * legendreP(l, 0, cosTheta) * normalization(l, 0);
return result;
}
Float SHVector::evalAzimuthallyInvariant(Float theta, Float phi) const {
Float result = 0, cosTheta = std::cos(theta);
for (int l=0; l<m_bands; ++l)
result += operator()(l, 0) * legendreP(l, 0, cosTheta) * normalization(l, 0);
return result;
}
void inliers::ransac()
{
int totalPointSize = firstImagePoints.size(); //'S' equals total set containing all points
//largest block to contain coefficients, accessed partially
float **coeffMat;
int num_pts = 2 * totalPointSize;
coeffMat = new float *[num_pts];
for(int i = 0; i < num_pts; i++)
coeffMat[i] = new float[9];
float outputMat[9];
Matrix3f homographyMat;
//select random points, but here three diverse points are selected
num_pts = 4;
Matrix3f mat, matD;
sampleInliers.clear();
sampleInliers.reserve(num_pts);
int interval = (totalPointSize / num_pts) - 1;
for(int j = 0; j < 4; j++)
sampleInliers.push_back(interval * j);
int dataID;
vector3d first, second;
bool ransacRepeat = true;
float threshold = 0.0; //'t' equals 3.84 * sigma_squared, maximum deviation to be an inlier
float errorDeviation[totalPointSize]; //deviation of point from mean, which is estimated to be zero
int oldSampleSize;
float oldOmega = 0.5; //increase of omega, which is prob. of inliers, suggests convergence
float omega;
int capitalN;
int ransacItr = 0;
while(ransacRepeat)
{
normalization(sampleInliers, mat, matD);
for(int i = 0; i < num_pts; i++)
{
dataID = sampleInliers[i];
first = global::firstImagePoints[dataID];
second = global::secondImagePoints[dataID];
first = mat * first;
second = matD * second;
coeffMat[2 * i][0] = 0;
coeffMat[2 * i][1] = 0;
coeffMat[2 * i][2] = 0;
coeffMat[2 * i][3] = -second.z * first.x;
coeffMat[2 * i][4] = -second.z * first.y;
coeffMat[2 * i][5] = -second.z * first.z;
coeffMat[2 * i][6] = second.y * first.x;
coeffMat[2 * i][7] = second.y * first.y;
coeffMat[2 * i][8] = second.y * first.z;
coeffMat[2 * i + 1][0] = second.z * first.x;
coeffMat[2 * i + 1][1] = second.z * first.y;
coeffMat[2 * i + 1][2] = second.z * first.z;
coeffMat[2 * i + 1][3] = 0;
coeffMat[2 * i + 1][4] = 0;
coeffMat[2 * i + 1][5] = 0;
coeffMat[2 * i + 1][6] = -second.x * first.x;
coeffMat[2 * i + 1][7] = -second.x * first.y;
coeffMat[2 * i + 1][8] = -second.x * first.z;
}
solutionSVD(coeffMat, num_pts * 2, 9, outputMat);
homographyMat = Matrix3f(outputMat);
matD.Inverse();
homographyMat = matD * homographyMat * mat;
//homographyMat.Display();
ransacItr++; //counter increased
//check accuracy of homographyMat with other points
for(int i = 0; i < totalPointSize; i++)
{
first = homographyMat * firstImagePoints[i];
first.change(first.x/first.z, first.y/first.z, 1.0);
second = secondImagePoints[i];
errorDeviation[i] = second.distancePointsSquared(first);
std::cout<<" index "<<i<<" and S.D. "<<sqrt(errorDeviation[i])<<" ";
//first.Display("f"); second.Display("s");
threshold += errorDeviation[i];
if( i % 2 == 0) std::cout<<std::endl;
}
threshold /= totalPointSize;
threshold = sqrt(threshold) * 0.8;
std::cout<<" thres "<<threshold<<std::endl;
oldSampleSize = sampleInliers.size(); //'s' is the sample Size, or old num_pts
sampleInliers.clear();
sampleInliers.reserve(totalPointSize);
std::cout<<std::endl;
for(int i = 0; i < totalPointSize; i++)
{
if(sqrt(errorDeviation[i]) <= threshold)
{
std::cout<<" Pindex "<<i<<" deviation "<<sqrt(errorDeviation[i])<<" ";
if( i % 2 == 0) std::cout<<std::endl;
sampleInliers.push_back(i); //'Si' is the consensus set
}
}
oldSampleSize = sampleInliers.size() - oldSampleSize;
std::cout<<std::endl<<" sampleInliersPushed "<<oldSampleSize<<" "<<std::endl;
num_pts = sampleInliers.size();
omega = (float)num_pts / totalPointSize; //probability of an inlier
omega = log10(1 - pow(omega, oldSampleSize));
capitalN = (-2)/omega;
std::cout<<std::endl<<" omega "<<omega<<" capitalN "<<capitalN<<std::endl;
if(capitalN <= ransacItr || ransacItr > 50) ransacRepeat = false;
//need to check whether omega was converging
}
std::cout<<std::endl;
//final re-estimation of the model
normalization(sampleInliers, mat, matD);
for(int i = 0; i < num_pts; i++)
{
dataID = sampleInliers[i];
first = global::firstImagePoints[dataID];
second = global::secondImagePoints[dataID];
first = mat * first;
second = matD * second;
coeffMat[2 * i][0] = 0;
coeffMat[2 * i][1] = 0;
coeffMat[2 * i][2] = 0;
coeffMat[2 * i][3] = -second.z * first.x;
coeffMat[2 * i][4] = -second.z * first.y;
coeffMat[2 * i][5] = -second.z * first.z;
coeffMat[2 * i][6] = second.y * first.x;
coeffMat[2 * i][7] = second.y * first.y;
coeffMat[2 * i][8] = second.y * first.z;
coeffMat[2 * i + 1][0] = second.z * first.x;
coeffMat[2 * i + 1][1] = second.z * first.y;
coeffMat[2 * i + 1][2] = second.z * first.z;
coeffMat[2 * i + 1][3] = 0;
coeffMat[2 * i + 1][4] = 0;
coeffMat[2 * i + 1][5] = 0;
coeffMat[2 * i + 1][6] = -second.x * first.x;
coeffMat[2 * i + 1][7] = -second.x * first.y;
coeffMat[2 * i + 1][8] = -second.x * first.z;
}
solutionSVD(coeffMat, num_pts * 2, 9, outputMat);
std::cout<<" answer "<<std::endl;
for(int j = 0; j < 9; j++)
std::cout<<" "<<outputMat[j]<<" ";
homographyMat = Matrix3f(outputMat);
homographyMat.Display("homoSmall");
matD.Inverse();
homographyMat = matD * homographyMat * mat;
homographyMat.Display("homography");
global::inliersRecord.clear();
global::inliersRecord.reserve(sampleInliers.size());
for(int i = 0; i < sampleInliers.size(); i++)
global::inliersRecord.push_back(sampleInliers[i]);
//release memory
delete[] coeffMat;
}
int main(int, char *[])
{
const std::string file = "/home/mathieu/dev/cochleo/share/cochlee.wav";
const audiofile audio(file);
filterbank fb(audio.sample_frequency(),100,6000,1000);
const auto xaxis = audio.time();
const auto yaxis = fb.center_frequency();
auto cochleo = fb.compute(audio.channel(0));
normalization(cochleo);
// Create the color palette
auto palette = vtkSmartPointer<vtkLookupTable>::New();
palette->SetTableRange(0.0,1.0);
palette->SetHueRange(0.67,0.);
palette->SetSaturationRange(0.7,0.3);
palette->Build();
// Create a c-style rgb image
const std::size_t width = xaxis.size();
const std::size_t height = yaxis.size();
std::vector<unsigned char> cImage(3*width*height);
for(std::size_t i=0; i<width; ++i)
for(std::size_t j=0; j<height; ++j)
{
unsigned char* color = palette->MapValue(cochleo[i][j]);
cImage[3*(i+width*j)] = color[0];
cImage[3*(i+width*j)+1] = color[1];
cImage[3*(i+width*j)+2] = color[2];
}
// Convert the c-style image to a vtkImageData
auto imageImport = vtkSmartPointer<vtkImageImport>::New();
imageImport->SetDataSpacing(0.07, 1, 1);
imageImport->SetDataOrigin(0, 0, 0);
imageImport->SetWholeExtent(0, width-1, 0, height-1, 0, 0);
imageImport->SetDataExtentToWholeExtent();
imageImport->SetDataScalarTypeToUnsignedChar();
imageImport->SetNumberOfScalarComponents(3);
imageImport->SetImportVoidPointer(cImage.data());
imageImport->Update();
// Create an actor
auto actor = vtkSmartPointer<vtkImageActor>::New();
actor->SetInput(imageImport->GetOutput());
// Create a scalar bar
auto scalarBar = vtkSmartPointer<vtkScalarBarActor>::New();
scalarBar->SetLookupTable(palette);
scalarBar->SetTitle("gain");
scalarBar->SetNumberOfLabels(5);
// Configure text of the scalar bar
auto textBar = vtkSmartPointer<vtkTextProperty>::New();
textBar->SetFontSize(10);
scalarBar->SetTitleTextProperty(textBar);
scalarBar->SetLabelTextProperty(textBar);
// Setup renderer
auto renderer = vtkSmartPointer<vtkRenderer>::New();
renderer->AddActor(actor);
renderer->AddActor(scalarBar);
renderer->ResetCamera();
// Setup render window
auto renderWindow = vtkSmartPointer<vtkRenderWindow>::New();
renderWindow->AddRenderer(renderer);
// Setup render window interactor
auto renderWindowInteractor = vtkSmartPointer<vtkRenderWindowInteractor>::New();
auto style = vtkSmartPointer<vtkInteractorStyleImage>::New();
renderWindowInteractor->SetInteractorStyle(style);
// Render and start interaction
renderWindowInteractor->SetRenderWindow(renderWindow);
renderWindowInteractor->Initialize();
renderWindowInteractor->Start();
return EXIT_SUCCESS;
}
void SpeedFiltration(float *V,float *vF)
{
TargSpeed.x = V[0];
TargSpeed.y = V[1];
TVector temp;
CurSpeed.x = vF[0];
CurSpeed.y = vF[1];
temp = subtraction(CurSpeed,TargSpeed);
ACCEL_INC = pow(mod(temp) * 10.0, 2) / 10.0 + 0.001 + mod(CurAccel) / 2.0;
if (ACCEL_INC > MAX_ACCEL_INC) { ACCEL_INC = MAX_ACCEL_INC;}
if (ACCEL_INC < -MAX_ACCEL_INC) { ACCEL_INC = -MAX_ACCEL_INC;}
TVector accelInc;
temp = subtraction(TargSpeed, CurSpeed);
temp = subtraction(temp, CurAccel);
temp = normalization(temp, ACCEL_INC);
accelInc = temp;
temp = subtraction(TargSpeed, CurSpeed);
temp = subtraction(CurAccel, temp );
temp = normalization(temp, ACCEL_INC);
TVector accelDec = temp;
TVector newAccelInc, newAccelDec;
newAccelInc = addition(CurAccel, accelInc);
newAccelDec = addition(CurAccel, accelDec);
TVector newSpeedInc, newSpeedDec, newSpeedZer;
newSpeedInc = addition(CurSpeed, newAccelInc);
newSpeedDec = addition(CurSpeed, newAccelDec);
newSpeedZer = addition(CurSpeed, CurAccel);
float timeToStop;
if (mod(accelInc) > 0)
timeToStop = abs(mod(CurAccel)/mod(accelInc))/2.0 + 0.50;
else timeToStop = 1;
float incErr = mod(subtraction(TargSpeed,addition(newSpeedInc,
addition(scale(newAccelInc,timeToStop),
scale(accelInc,timeToStop*timeToStop/2.0)))));
float decErr = mod(subtraction(TargSpeed,addition(newSpeedDec,
addition(scale(newAccelDec,timeToStop),
scale(accelDec,timeToStop*timeToStop/2.0)))));
float zerErr = mod(subtraction(TargSpeed,addition(newSpeedDec,
scale(newAccelDec,timeToStop))));
if (incErr > decErr)
{
if (decErr < zerErr)
{
AccelInc1 = accelDec;
CurSpeed = newSpeedDec;
CurAccel = newAccelDec;
}
else
{
AccelInc1.x = 0;
AccelInc1.y = 0;
CurSpeed = newSpeedZer;
}
}
else
{
if (incErr < zerErr) {
AccelInc1 = accelInc;
CurSpeed = newSpeedInc;
CurAccel = newAccelInc;
}
else
{
AccelInc1.x = 0;
AccelInc1.y = 0;
CurSpeed =newSpeedZer;
}
}
if (mod(CurAccel) > MAX_ACCEL)
CurAccel = normalization(CurAccel, MAX_ACCEL);
vF[0] = CurSpeed.x;
vF[1] = CurSpeed.y;
vF[2] = V[2];
}
int main(int argc, char** argv){
if(argc < 2){
printf("pass in the file name as the first argument\n");
std::cin.get();
exit(1);
}
IplImage* img = cvLoadImage(argv[1], CV_LOAD_IMAGE_GRAYSCALE);
cvNamedWindow("MIT Road Paint Detector", CV_WINDOW_AUTOSIZE);
cvSetMouseCallback("MIT Road Paint Detector", mouseCallbackFunc);
/* camera matrix set up */
Camera camera(FOCAL_LENGTH, 640, 480);
CvMat *P = camera.getP();
CvPoint2D32f point = camera.imageToGroundPlane(cvPoint(1,1));
printf("x = %f, %f, \n", point.x, point.y);
float Y;
int row, col, j, k, width;
std::vector<float> kernel;
std::vector<float> out(640);
std::vector<int> in(640);
Convolution convolution;
FILE *fp;
for (row = 0; row < img->height; row++) {
Y = calculateY(P, row);
width = calculateWidthInPixels(P, Y);
if (width < ONE_PIXEL) {
width = 0;
}
#if defined(DEBUG)
if (row == 374) {
fp = fopen("input.txt", "wt");
fprintf(fp, "#\t X\t Y\n");
}
#endif
for (col = 0; col < img->width; col++) {
CvScalar scalar = cvGet2D(img, row, col);
in[col] = scalar.val[0];
#if defined(DEBUG)
if (row == 374) {
fprintf(fp, "\t %d\t %d\t\n", col, in[col]);
}
#endif
}
#if defined(DEBUG)
if (row == 374) {
fclose(fp);
}
#endif
//find the kernel only if only there is a width
if (width > 0) {
convolution.kernel1D(width, kernel);
convolution.convolve1D(in, kernel, out);
#if defined(DEBUG)
if (row == 374) {
//convolution result
fp = fopen("convolution.txt", "wt");
fprintf(fp, "#\t X\t Y\n");
for (col = 0; col < img->width; col++) {
fprintf(fp, "\t %d\t %f\n", col, out[col]);
}
fclose(fp);
//kernel result
fp = fopen("kernel.txt", "wt");
fprintf(fp, "#\t X\t Y\n");
int left = in.size() - kernel.size() / 2;
int right = left + 1;
for (col = 0; col < left; col++) {
fprintf(fp, "\t %d\t %f\n", col, 0.0);
}
k = col;
for (col = 0; col < kernel.size(); col++) {
fprintf(fp, "\t %d\t %f\n", k++, kernel[i] * 255);
}
for (col = 0; col < right; col++) {
fprintf(fp, "\t %d\t %f\n", k++, 0.0);
}
fclose(fp);
}
#endif
normalization(out, img->width, width);
localMaximaSuppression(out, img->width);
}
else {
out.resize(img->width);
for (col = 0; col < img->width; col++) {
out[col] = 0;
}
}
for (col = 0; col < img->width; col++) {
CvScalar scalar;
scalar.val[0] = out[col];
cvSet2D(img, row, col, scalar);
}
} // end for loop
Contour contour;
contour.findContours(img, camera);
img = contour.drawContours();
cvShowImage("MIT Road Paint Detector", img);
cvWaitKey(0);
cvReleaseImage(&img);
cvDestroyWindow("MIT Read Paint Detector");
return 0;
}
void Scwt_squeezed(float *input, double *squeezed_r,
double *squeezed_i, int *pnboctave, int *pnbvoice,
int *pinputsize, float *pcenterfrequency)
{
int nboctave, nbvoice, i, j, inputsize, bigsize;
float centerfrequency, a;
double *Ri2, *Ri1, *Ii1, *Ii2, *Rdi2, *Idi2, *Ii, *Ri;
double *Oreal, *Oimage, *Odreal, *Odimage;
centerfrequency = *pcenterfrequency;
nboctave = *pnboctave;
nbvoice = *pnbvoice;
inputsize = *pinputsize;
bigsize = inputsize*nbvoice*nboctave;
/* Memory allocations
------------------*/
if(!(Oreal = (double *)calloc(bigsize, sizeof(double))))
error("Memory allocation failed for Ri1 in cwt_phase.c \n");
if(!(Oimage = (double *)calloc(bigsize, sizeof(double))))
error("Memory allocation failed for Ii1 in cwt_phase.c \n");
if(!(Odreal = (double *)calloc(bigsize, sizeof(double))))
error("Memory allocation failed for Ri1 in cwt_phase.c \n");
if(!(Odimage = (double *)calloc(bigsize, sizeof(double))))
error("Memory allocation failed for Ii1 in cwt_phase.c \n");
if(!(Ri1 = (double *)calloc(inputsize, sizeof(double))))
error("Memory allocation failed for Ri1 in cwt_phase.c \n");
if(!(Ii1 = (double *)calloc(inputsize, sizeof(double))))
error("Memory allocation failed for Ii1 in cwt_phase.c \n");
if(!(Ii2 = (double *)calloc(inputsize,sizeof(double))))
error("Memory allocation failed for Ri2 in cwt_phase.c \n");
if(!(Ri2 = (double *)calloc(inputsize,sizeof(double))))
error("Memory allocation failed for Ri2 in cwt_phase.c \n");
if(!(Idi2 = (double *)calloc(inputsize,sizeof(double))))
error("Memory allocation failed for Ri2 in cwt_phase.c \n");
if(!(Rdi2 = (double *)calloc(inputsize,sizeof(double))))
error("Memory allocation failed for Ri2 in cwt_phase.c \n");
if(!(Ri = (double *)calloc(inputsize, sizeof(double))))
error("Memory allocation failed for Ri in cwt_phase.c \n");
if(!(Ii = (double *)calloc(inputsize, sizeof(double))))
error("Memory allocation failed for Ii in cwt_phase.c \n");
for(i = 0; i < inputsize; i++){
*Ri = (double)(*input);
Ri++; input++;
}
Ri -= inputsize;
input -= inputsize;
/* Compute fft of the signal
-------------------------*/
double_fft(Ri1,Ii1,Ri,Ii,inputsize,-1);
/* Multiply signal and wavelets in the Fourier space
-------------------------------------------------*/
for(i = 1; i <= nboctave; i++) {
for(j=0; j < nbvoice; j++) {
a = (float)(pow((double)2,(double)(i+j/((double)nbvoice))));
morlet_frequencyph(centerfrequency,a,Ri2,Idi2,inputsize);
multiply(Ri1,Ii1,Ri2,Ii2,Oreal,Oimage,inputsize);
multiply(Ri1,Ii1,Rdi2,Idi2,Odreal,Odimage,inputsize);
double_fft(Oreal,Oimage,Oreal,Oimage,inputsize,1);
double_fft(Odreal,Odimage,Odreal,Odimage,inputsize,1);
Oreal += inputsize;
Oimage += inputsize;
Odreal += inputsize;
Odimage += inputsize;
}
}
Oreal -= bigsize;
Odreal -= bigsize;
Oimage -= bigsize;
Odimage -= bigsize;
free((char *)Ri2);
free((char *)Ri1);
free((char *)Ii1);
free((char *)Ii2);
free((char *)Ri);
free((char *)Ii);
/* Normalize the cwt and compute the squeezed transform
----------------------------------------------------*/
normalization(Oreal, Oimage, Odreal, Odimage, bigsize);
w_reassign(Oreal, Oimage, Odreal, Odimage, squeezed_r,
squeezed_i, centerfrequency,inputsize, nbvoice, nboctave);
return;
}
void Scwt_phase(double *input, double *Oreal, double *Oimage,
double *f, int *pnboctave, int *pnbvoice, int *pinputsize,
double *pcenterfrequency)
{
int nboctave, nbvoice, i, j, inputsize;
double centerfrequency, a;
double *Ri2, *Ri1, *Ii1, *Ii2, *Rdi2, *Idi2, *Ii, *Ri;
double *Odreal, *Odimage;
centerfrequency = *pcenterfrequency;
nboctave = *pnboctave;
nbvoice = *pnbvoice;
inputsize = *pinputsize;
/* Memory allocations --
is the original use of calloc significant??
Using S_alloc to initialize mem, just in case. note by xian
------------------*/
if(!(Odreal = (double *) S_alloc(inputsize*nbvoice*nboctave, sizeof(double))))
Rf_error("Memory allocation failed for Ri1 in cwt_phase.c \n");
if(!(Odimage = (double *) S_alloc(inputsize*nbvoice*nboctave, sizeof(double))))
Rf_error("Memory allocation failed for Ii1 in cwt_phase.c \n");
if(!(Ri1 = (double *) S_alloc(inputsize, sizeof(double))))
Rf_error("Memory allocation failed for Ri1 in cwt_phase.c \n");
if(!(Ii1 = (double *) S_alloc(inputsize, sizeof(double))))
Rf_error("Memory allocation failed for Ii1 in cwt_phase.c \n");
if(!(Ii2 = (double *) S_alloc(inputsize,sizeof(double))))
Rf_error("Memory allocation failed for Ri2 in cwt_phase.c \n");
if(!(Ri2 = (double *) S_alloc(inputsize,sizeof(double))))
Rf_error("Memory allocation failed for Ri2 in cwt_phase.c \n");
if(!(Idi2 = (double *) S_alloc(inputsize,sizeof(double))))
Rf_error("Memory allocation failed for Ri2 in cwt_phase.c \n");
if(!(Rdi2 = (double *) S_alloc(inputsize,sizeof(double))))
Rf_error("Memory allocation failed for Ri2 in cwt_phase.c \n");
if(!(Ri = (double *) S_alloc(inputsize, sizeof(double))))
Rf_error("Memory allocation failed for Ri in cwt_phase.c \n");
if(!(Ii = (double *) S_alloc(inputsize, sizeof(double))))
Rf_error("Memory allocation failed for Ii in cwt_phase.c \n");
for(i = 0; i < inputsize; i++){
*Ri = (double)(*input);
Ri++; input++;
}
Ri -= inputsize;
input -= inputsize;
/* Compute fft of the signal
-------------------------*/
double_fft(Ri1,Ii1,Ri,Ii,inputsize,-1);
/* Multiply signal and wavelets in the Fourier space
-------------------------------------------------*/
for(i = 1; i <= nboctave; i++) {
for(j=0; j < nbvoice; j++) {
a = (double)(pow((double)2,(double)(i+j/((double)nbvoice))));
morlet_frequencyph(centerfrequency,a,Ri2,Idi2,inputsize);
multiply(Ri1,Ii1,Ri2,Ii2,Oreal,Oimage,inputsize);
multiply(Ri1,Ii1,Rdi2,Idi2,Odreal,Odimage,inputsize);
double_fft(Oreal,Oimage,Oreal,Oimage,inputsize,1);
double_fft(Odreal,Odimage,Odreal,Odimage,inputsize,1);
Oreal += inputsize;
Oimage += inputsize;
Odreal += inputsize;
Odimage += inputsize;
}
}
Oreal -= inputsize*nbvoice*nboctave;
Odreal -= inputsize*nbvoice*nboctave;
Oimage -= inputsize*nbvoice*nboctave;
Odimage -= inputsize*nbvoice*nboctave;
/* Normalize the cwt and compute the f function
--------------------------------------------*/
normalization(Oreal, Oimage, Odreal, Odimage,
inputsize*nbvoice*nboctave);
f_function(Oreal, Oimage, Odreal, Odimage, f,
centerfrequency,inputsize,nbvoice,nboctave);
return;
}
cv::Point3d Camera::normalize(const cv::Point2d p) const
{
cv::Mat_<double> normalization = _inverse * (cv::Mat_<double>(3, 1) << p.x, p.y, 1.0);
return cv::Point3d(normalization(0), normalization(1), normalization(2));
}
