I am using 5 kinect in pentagon to capture whole body 360°
I have already got quaternion by using colmap.
But it can't transform the .ply file correctly.
I think the cause of the problem is kinect is a right hand camera
Here is my code
float qw1 = 0.980613, qx1 = -0.0777902, qy1 = -0.176786, qz1 = -0.0330758, tx1 = -0.798112, ty1 = -0.774293, tz1 = 3.76053;
float qw2 = 0.861117, qx2 = -0.0716478, qy2 = 0.427619, qz2 = 0.265493, tx2 = -2.94326, ty2 = -1.91445, tz2 = 6.074;
float qw3 = 0.954216, qx3 = -0.0519415, qy3 = -0.265356, qz3 = -0.127908, tx3 = 0.983777, ty3 = 0.099799, tz3 = 1.74569;
float qw4 = 0.977833, qx4 = -0.0748412, qy4 = 0.148912, qz4 = 0.126758, tx4 = -1.72221, ty4 = -0.292148, tz4 = 2.7061;
float qw5 = 0.771623, qx5 = -0.0842691, qy5 = -0.547153, qz5 = -0.313242, tx5 = 2.53668, ty5 = -0.921194, tz5 = 3.55139;
//master
Eigen::Quaternionf quat1(qw1, qx1, qy1, qz1);
Eigen::Matrix4f transform_matrix1 = Eigen::Matrix4f::Identity();
Eigen::Matrix3f rot_mat1 = quat1.toRotationMatrix();
transform_matrix1.block(0, 0, 3, 3) = rot_mat1;
transform_matrix1.block(0, 3, 3, 1) << tx1, ty1, tz1;
std::cout << "Master : " << std::endl;
std::cout << transform_matrix1 << std::endl;
//sub01
Eigen::Quaternionf quat2(qw2, qx2, qy2, qz2);
Eigen::Matrix4f transform_matrix2 = Eigen::Matrix4f::Identity();
Eigen::Matrix3f rot_mat2 = quat2.toRotationMatrix();
transform_matrix2.block(0, 0, 3, 3) = rot_mat2;
transform_matrix2.block(0, 3, 3, 1) << tx2, ty2, tz2;
std::cout << "Sub01 : " << std::endl;
std::cout << transform_matrix2 << std::endl;
//sub02
Eigen::Quaternionf quat3(qw3, qx3, qy3, qz3);
Eigen::Matrix4f transform_matrix3 = Eigen::Matrix4f::Identity();
Eigen::Matrix3f rot_mat3 = quat3.toRotationMatrix();
transform_matrix3.block(0, 0, 3, 3) = rot_mat3;
transform_matrix3.block(0, 3, 3, 1) << tx3, ty3, tz3;
std::cout << "Sub02 : " << std::endl;
std::cout << transform_matrix3 << std::endl;
......
pcl::transformPointCloud(*cloud_master, *master_transformed, transform_matrix1);
pcl::transformPointCloud(*cloud_sub01, *sub01_transformed, transform_matrix2);
pcl::transformPointCloud(*cloud_sub02, *sub02_transformed, transform_matrix3);
pcl::transformPointCloud(*cloud_sub03, *sub03_transformed, transform_matrix4);
pcl::transformPointCloud(*cloud_sub04, *sub04_transformed, transform_matrix5);
Related
I am still learning about threads and I was trying to solve this problem in my code, when I am putting the pthread_join(thread[i],NULL) outside the loop that is creating the threads it always gives me wrong output and Thread with ID = 0 will not work(call the median func) and the last thread will work two times, for better understanding see the output below:
ThreadID= 0, startRow= 0, endRow= 0 // first thread doesn't call the median func
ThreadID= 1, startRow= 1, endRow= 1
ThreadID 1 numOfBright 0 numOfDark 1 numOfNormal 4
ThreadID= 2, startRow= 2, endRow= 2
ThreadID 2 numOfBright 0 numOfDark 1 numOfNormal 4
ThreadID= 3, startRow= 3, endRow= 3
ThreadID 3 numOfBright 0 numOfDark 0 numOfNormal 5
ThreadID= 4, startRow= 4, endRow= 4
ThreadID 4 numOfBright 0 numOfDark 5 numOfNormal 0
ThreadID 4 numOfBright 0 numOfDark 5 numOfNormal 0 // last thread is calling the median func two times.
This is the part of the code that prints the start and end row of each thread.
pthread_t* threads = new pthread_t[num_threads];
struct Th_Range* RANGE = (struct Th_Range*)malloc(sizeof(struct Th_Range*));
int thread_status;
RANGE->SizeOfImage = r; // 2d array with size (n*n) so rows(r) = columns(c)
if (n == num_threads) { //rows = num of threads then every thread will work in a single row
for (int i = 0; i < num_threads; i++) {
RANGE->ThreadId = i;
RANGE->StartRow = RANGE->EndRow = i;
cout << "ThreadID= " << i << ", startRow= " << RANGE->StartRow << ", endRow= " << RANGE->EndRow << endl;
thread_status = pthread_create(&threads[i], NULL, Median, RANGE);
if (thread_status)
exit(-1);
} //for loop ends here
for (int i = 0; i < num_threads; i++)
pthread_join(threads[i],NULL);
} //end of if statement
Here is the part of the code if needed with the median function and the above if statement.
#include <iostream>
#include <bits/stdc++.h>
#include <pthread.h>
pthread_mutex_t Lock;
pthread_mutex_t Pixels;
pthread_mutex_t Pixels2;
using namespace std;
int numOfBright, numOfDark, numOfNormal;
int** Oimage, ** Fimage; //original and filtered image
struct Th_Range {
int SizeOfImage;
int StartRow;
int EndRow;
int ThreadId;
};
void* Median(void* par)
{
struct Th_Range* Num = (struct Th_Range*)par;
int StartRow = Num->StartRow;
int EndRow = Num->EndRow;
int Size = Num->SizeOfImage;
int Neighbour[9] = { 0 };
int dark = 0, bright = 0, normal = 0;
if (EndRow == StartRow)
EndRow += 2;
else
EndRow++;
for (int i = StartRow +1; i < EndRow ; i++)
{
for (int j = 1; j < Size - 1; j++)
{
Neighbour[0] = Oimage[i - 1][j - 1];
Neighbour[1] = Oimage[i - 1][j];
Neighbour[2] = Oimage[i - 1][j + 1];
Neighbour[3] = Oimage[i][j - 1];
Neighbour[4] = Oimage[i][j];
Neighbour[5] = Oimage[i][j + 1];
Neighbour[6] = Oimage[i + 1][j - 1];
Neighbour[7] = Oimage[i + 1][j];
Neighbour[8] = Oimage[i + 1][j + 1];
pthread_mutex_lock(&Pixels); //it can be moved only to lock the Fimage and the numOfBright or any other global variables
sort(Neighbour, Neighbour + 9);
Fimage[i][j] = Neighbour[4];
if (Neighbour[4] > 200) {
bright++;
numOfBright++;
}
else if (Neighbour[4] < 50) {
dark++;
numOfDark++;
}
else {
normal++;
numOfNormal++;
}
pthread_mutex_unlock(&Pixels);
}
}
pthread_mutex_lock(&Pixels2); //when I try to remove this lock the output gets interrupted
cout << "ThreadID " << Num->ThreadId << " numOfBright " << bright << " numOfDark " << dark << " numOfNormal " << normal<<endl;
pthread_mutex_unlock(&Pixels2);
pthread_exit(NULL);
}
int main(int argc, char* argv[])
{
int num_threads, n, r, c; // n is the size of the matrix r and c are rows and columns
numOfNormal = numOfDark = numOfBright = 0;
if (argc >= 2)
num_threads = atoi(argv[1]);
else
exit(-1);
ifstream cin("input.txt");
cin >> n;
r = c = n + 2;
Oimage = new int* [r]();
Fimage = new int* [r]();
for (int i = 0; i < c; i++)
{
Oimage[i] = new int[c]();
Fimage[i] = new int[c]();
}
for (int i = 1; i < r - 1; i++)
for (int j = 1; j < c - 1; j++)
cin >> Oimage[i][j];
pthread_t* threads = new pthread_t[num_threads];
struct Th_Range* RANGE = (struct Th_Range*)malloc(sizeof(struct Th_Range*));
RANGE->SizeOfImage = r;
if (n == num_threads) { //rows = num of threads then every thread will work in a single row
//n+2
int thread_status;
for (int i = 0; i < num_threads; i++) {
RANGE->ThreadId = i;
RANGE->StartRow = RANGE->EndRow = i;
// pthread_mutex_lock(&Lock);
cout << "ThreadID= " << i << ", startRow= " << RANGE->StartRow << ", endRow= " << RANGE->EndRow << endl;
thread_status = pthread_create(&threads[i], NULL, Median, RANGE);
if (thread_status)
exit(-1);
}
}
I tried to move pthread_join inside the loop of pthread_create it gives a correct output but of course it is a wrong solution. I have no idea what to do next. Thanks in advance
Maybe you should use #include
or (using namespace sff) it must work
I am trying to solve a very simple task tested by my univeristy's code checker. The code is about a c++ implementation of the equation of the quadratic equation. The code won't work for all cases, I am supplying my code, and if there is some comment, hint, please help me with it.
Code:
#include <iostream>
#include <math.h>
using namespace std;
int main()
{
freopen("input.txt", "r", stdin);
freopen("output.txt", "w", stdout);
int a, b, c, delta;
double x1, x2, x;
cin >> a >> b >> c;
delta = ((b*b) - (4 * a * c));
if (a == 0) {
if (b != 0) {
x = (-(c*1.0) / (b));
cout << 1 << " " << x;
}
else
cout << "-1";
}
else
if (delta > 0)
{
x1 = ((-(b + sqrt(delta))*1.0) / (2 * a));
x2 = ((-(b - sqrt(delta))*1.0) / (2 * a));
cout << 2 << " " << x1 << " " << x2;
}
else if (delta == 0)
{
x = (-(b*1.0) / (2 * a));
cout << 1 << " " << x;
}
else
cout << "-1";
return 0;
}
I am taking hard coded information of two arrays and creating the Union, Intersection and Difference of them.
My issue is that I can't get the Intersection or Difference to print with the right format.
I have tried a different variety of while loops with for and if statements, but can't quite get it. I know I'm close-ish. (Hopefully)
Main information:
int setA[] = {3,4,9,12,13,15};
int setB[] = {1,3,5,7,9};
int lenA = sizeof(setA) / sizeof(setA[0]);
int lenB = sizeof(setB) / sizeof(setB[0]);
Difference(setA, setB, lenA, lenB);
Intersection(setA, setB, lenA, lenB);
void Intersection(int setA[], int setB[], int lenA, int lenB)
{
cout << "AnB = {";
for (int i = 0; i < lenA; i++)
{
for (int j = 0; j < lenB; j++)
{
while(setA[i] == setB[j] && i < lenA - 1)
{
cout << setA[i++] << ", ";
}
if(i < lenA && setA[i] == setB[j])
{
cout << setA[i++];
}
}
}
cout << "}";
cout << endl;
}
void Difference(int setA[], int setB[], int lenA, int lenB)
{
cout << "A - B = {";
for (int i = 0; i < lenA; i++)
{
bool comp = false;
for (int j = 0; j < lenB; j++)
if (setA[i] == setB[j])
{
comp = true;
break;
}
if(!comp)
{
cout << setA[i] << ",";
}
}
cout << "}";
cout << endl;
}
I want the output to look like:
AnB = {3, 9} (Intersection)
A-B = {4, 12, 13, 15} (Difference)
What I am getting:
AnB = {3, 9, } (Intersection)
A-B = {4, 12, 13, 15, } (Difference)
I just need to get the commas at the end fixed, but don't know how.
Sorry if this question is way too long, I wasn't sure of how to get my issue out.
I am dealing with generation of tool path where composed many points in three dimension and I am using CNC machine to generate them. One of the things that I want to calculate is tool path length which is defined the total length of path. So I tried this:
1.6760 3.7901 6.1955
1.2788 4.1872 5.3681
0.2832 5.1828 3.2939
0.1835 5.2173 3.0576
0.1097 5.1205 2.8292
0.0815 4.9185 2.6699
0.0812 4.8728 2.6491
0.0810 4.8270 2.6288
0.0807 4.7810 2.6089
The points are like these.
// math.cpp : Defines the entry point for the console application.
//
#include "stdafx.h"
#include <iostream>
#include <fstream>
#include <vector>
#include<math.h>
using std::cin;
using std::cout;
using std::endl;
using std::vector;
using std::ostream;
using std::istream;
using std::ifstream;
using std::operator>>;
using std::operator<<;
struct point
{
float x ;
float y ;
float z ;
};
ostream& operator<< (ostream& out, const point &p)
{
out << "(" << p.x << "," << p.y << " ," << p.z << "," << ")";
return out;
}
istream& operator>> (istream& in, point& point)
{
in >> point.x >> point.y >> point.z;
return in;
}
struct line
{
point start;
point next;
float sqDistance()
{
float dx = start.x - next.x;
float dy = start.y - next.y;
float dz = start.z - next.z;
double distance = 0.0;
distance = sqrt(dx * dx + dy * dy + dz * dz);
return distance;
}
};
ostream& operator<< (ostream& out, const line &ln)
{
out << "From " << ln.start << " to " << ln.next;
return out;
}
istream& operator>> (istream& in, line ln)
{
cout << "Enter x y z start then x y z next: ";
in >> ln.start.x >> ln.start.y >> ln.start.z >> ln.next.x >> ln.next.y >> ln.next.z;
return in;
}
int main()
{
point origin, input;
line ray;
vector<line> side;
// READ POINTS FROM FILE
ifstream pointfile("concave.txt");
if (pointfile.is_open())
{
pointfile >> origin.x >> origin.y >> origin.z;
cout << "origin: " << origin << endl;
ray.start = origin;
while (pointfile >> ray.next)
{
cout
<< " GOTO/ " << ray.next
<< " The distance from point to the next is : "
<< ray.sqDistance() << endl;
side.push_back(ray);
}
}
else
cout << "Unable to open file";
pointfile.close();
vector<line>::iterator iter = side.begin();
line temp, closest = *iter;
float minimumDistance = closest.sqDistance(), distance = 0.0;
system("PAUSE");
return 0;
}
-I expect the distance between point and its next point.
-the total length of this line.
What about something like this - suppose you want distance between points and not from start (line 93 with comment):
#include "stdafx.h"
#include <iostream>
#include <fstream>
#include <vector>
#include<math.h>
#include <conio.h>
using std::cin;
using std::cout;
using std::endl;
using std::vector;
using std::ostream;
using std::istream;
using std::ifstream;
using std::operator>>;
using std::operator<<;
struct point
{
float x ;
float y ;
float z ;
};
ostream& operator<< (ostream& out, const point &p)
{
out << "(" << p.x << "," << p.y << " ," << p.z << "," << ")";
return out;
}
istream& operator>> (istream& in, point& point)
{
in >> point.x >> point.y >> point.z;
return in;
}
struct line
{
point start;
point next;
float sqDistance()
{
float dx = start.x - next.x;
float dy = start.y - next.y;
float dz = start.z - next.z;
double distance = 0.0;
distance = sqrt(dx * dx + dy * dy + dz * dz);
return distance;
}
};
ostream& operator<< (ostream& out, const line &ln)
{
out << "From " << ln.start << " to " << ln.next;
return out;
}
istream& operator>> (istream& in, line ln)
{
cout << "Enter x y z start then x y z next: ";
in >> ln.start.x >> ln.start.y >> ln.start.z >> ln.next.x >> ln.next.y >> ln.next.z;
return in;
}
int main()
{
point origin, input;
line ray;
vector<line> side;
// READ POINTS FROM FILE
ifstream pointfile("concave.txt");
if (pointfile.is_open())
{
pointfile >> origin.x >> origin.y >> origin.z;
cout << "origin: " << origin << endl;
ray.start = origin;
while (pointfile >> ray.next)
{
cout
<< " GOTO/ " << ray.next
<< " The distance from point to the next is : "
<< ray.sqDistance() << endl;
side.push_back(ray);
ray.start = ray.next; // set start to last end (?)
}
}
else
cout << "Unable to open file";
pointfile.close();
vector<line>::iterator iter = side.begin();
line temp, closest = *iter;
float minimumDistance = closest.sqDistance(), distance, sumDistance = 0.0;
cout << "Line coords" << endl << "distance, Sum of distances, minimum" << endl;
while(iter != side.end()) {
closest = *iter;
distance = closest.sqDistance();
sumDistance += distance;
if(minimumDistance > distance) minimumDistance = distance;
cout << closest << endl
<< distance << " | " << sumDistance << " | " << minimumDistance << endl;
sumDistance += distance;
iter++;
}
getch();
return 0;
}
Sorry for the bug - sum line was there twice - line after cout was an error, also noticed some precision warning - mixed double/float, so switched to double everywhere, now main loop looks like this:
vector<line>::iterator iter = side.begin();
line closest = *iter;
double distance, sumOfDistances = 0.0;
cout << "Line coords" << endl << "distance | Sum of distances" << endl;
while (iter != side.end()) {
closest = *iter;
distance = closest.sqDistance();
sumOfDistances += distance;
cout << closest << endl << distance << " | " << sumOfDistances << endl; // step info output
iter++;
}
Here complete, a bit shorter version including simple results.txt file output.
You can remove 2 lines with comment info output:
#include <iostream>
#include <fstream>
#include <vector>
#include <conio.h>
using namespace std;
struct point
{
float x;
float y;
float z;
};
ostream& operator<< (ostream& out, const point &p)
{
out << "(" << p.x << "," << p.y << " ," << p.z << "," << ")";
return out;
}
istream& operator>> (istream& in, point& point)
{
in >> point.x >> point.y >> point.z;
return in;
}
struct line
{
point start;
point next;
double sqDistance()
{
float dx = start.x - next.x;
float dy = start.y - next.y;
float dz = start.z - next.z;
double distance = 0.0;
distance = sqrt(dx * dx + dy * dy + dz * dz);
return distance;
}
};
ostream& operator<< (ostream& out, const line &ln)
{
out << "From " << ln.start << " to " << ln.next;
return out;
}
istream& operator>> (istream& in, line ln)
{
cout << "Enter x y z start then x y z next: ";
in >> ln.start.x >> ln.start.y >> ln.start.z >> ln.next.x >> ln.next.y >> ln.next.z;
return in;
}
int main()
{
point origin, input;
line ray;
vector<line> side;
// READ POINTS FROM FILE
ifstream pointfile("concave.txt");
if (pointfile.is_open())
{
pointfile >> origin.x >> origin.y >> origin.z;
cout << "origin: " << origin << endl;
ray.start = origin;
while (pointfile >> ray.next)
{
cout
<< " GOTO/ " << ray.next
<< " The distance from point to the next is : "
<< ray.sqDistance() << endl;
side.push_back(ray);
ray.start = ray.next; // set start to last end (?)
}
}
else
cout << "Unable to open file";
pointfile.close();
ofstream results("results.txt");
vector<line>::iterator iter = side.begin();
line closest = *iter;
double distance, sumOfDistances = 0.0;
cout << "Line coords" << endl << "distance | Sum of distances" << endl;
while (iter != side.end()) {
closest = *iter;
distance = closest.sqDistance();
sumOfDistances += distance;
results << distance << endl;
cout << closest << endl << distance << " | " << sumOfDistances << endl; // info output
iter++;
}
results << sumOfDistances << " << Sum" << endl;
results.close();
cout << "Complete path distance: " << sumOfDistances << endl; // info output
getch();
return 0;
}
When I try to build the solution for this program I am receiving the error End1 Not declared for all the end1 statements. Am I missing something?
#include "StdAfx.h
#include <iostream>
using namespace std;
const double PI = 3.14;
int main()
{
double circumference;
double radius;
double area;
cout << "Enter the radius: ";
cin >> radius;
cout << end1;
circumference = 2 * PI * radius;
cout << "Circumference = " << circumference << end1;
area = PI * radius * radius;
cout << "Area = " << area << end1;
return 0;
}
Try replacing the number 1 with the letter l
http://en.cppreference.com/w/cpp/io/manip/endl
replace end1 with endl
cout << "Circumference = " << circumference << endl;