-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.cpp
165 lines (133 loc) · 3.79 KB
/
main.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
#include <iostream>
#include <sstream>
#include <string>
#include <fstream>
#include <iomanip>
#include "Eigen/Dense"
#include <cmath>
#include <math.h>
#include "Lab9Structures.h"
#include "MatrixIOFunctions.h"
using namespace std;
using namespace Eigen;
int main()
{
//Reading data from txt file.
MatrixXd PointCloud = ReadDataToMatrix("PointCloudData.txt");
int r = PointCloud.rows();
int c = PointCloud.cols();
// Empty vector of Plane objects
vector <Plane> planeobjects;
// Matrix I with n x 1
MatrixXd I;
I.resize(r, 1);
//Setting to Zero
I.setZero();
//Part d. of the lab
//d.a
//Reading all Seeds files and putting into a matrix.
MatrixXd Seeds1 = ReadDataToMatrix("Seeds1.txt");
MatrixXd Seeds2 = ReadDataToMatrix("Seeds2.txt");
MatrixXd Seeds3 = ReadDataToMatrix("Seeds3.txt");
MatrixXd Seeds4 = ReadDataToMatrix("Seeds4.txt");
MatrixXd Seeds5 = ReadDataToMatrix("Seeds5.txt");
MatrixXd Seeds6 = ReadDataToMatrix("Seeds6.txt");
// Matching Seeds1.txt to points in Point Cloud
MatrixXd Match1(Seeds1.rows(), 4);
for (int i = 0; i < Seeds1.rows(); i++)
{
for (int a = 0; a < r; a++)
{
if (Seeds1(i, 0) == PointCloud(a, 0))
{
Match1(i,0) = PointCloud(a, 0);
Match1(i,1) = PointCloud(a, 1);
Match1(i,2) = PointCloud(a, 2);
Match1(i,3) = PointCloud(a, 3);
}
}
}
// Matching Seeds2.txt to points in Point Cloud
MatrixXd Match2(Seeds2.rows(), 4);
for (int i = 0; i < Seeds2.rows(); i++)
{
for (int a = 0; a < r; a++)
{
if (Seeds2(i, 0) == PointCloud(a, 0))
{
Match2(i,0) = PointCloud(a, 0);
Match2(i,1) = PointCloud(a, 1);
Match2(i,2) = PointCloud(a, 2);
Match2(i,3) = PointCloud(a, 3);
}
}
}
// Matching Seeds3.txt to points in Point Cloud
MatrixXd Match3(Seeds3.rows(), 4);
for (int i = 0; i < Seeds3.rows(); i++)
{
for (int a = 0; a < r; a++)
{
if (Seeds3(i, 0) == PointCloud(a, 0))
{
Match3(i,0) = PointCloud(a, 0);
Match3(i,1) = PointCloud(a, 1);
Match3(i,2) = PointCloud(a, 2);
Match3(i,3) = PointCloud(a, 3);
}
}
}
// Matching Seeds4.txt to points in Point Cloud
MatrixXd Match4(Seeds4.rows(), 4);
for (int i = 0; i < Seeds4.rows(); i++)
{
for (int a = 0; a < r; a++)
{
if (Seeds4(i, 0) == PointCloud(a, 0))
{
Match4(i,0) = PointCloud(a, 0);
Match4(i,1) = PointCloud(a, 1);
Match4(i,2) = PointCloud(a, 2);
Match4(i,3) = PointCloud(a, 3);
}
}
}
// Matching Seeds5.txt to points in Point Cloud
MatrixXd Match5(Seeds5.rows(), 4);
for (int i = 0; i < Seeds5.rows(); i++)
{
for (int a = 0; a < r; a++)
{
if (Seeds5(i, 0) == PointCloud(a, 0))
{
Match5(i,0) = PointCloud(a, 0);
Match5(i,1) = PointCloud(a, 1);
Match5(i,2) = PointCloud(a, 2);
Match5(i,3) = PointCloud(a, 3);
}
}
}
// Matching Seeds6.txt to points in Point Cloud
MatrixXd Match6(Seeds6.rows(), 4);
for (int i = 0; i < Seeds6.rows(); i++)
{
for (int a = 0; a < r; a++)
{
if (Seeds6(i, 0) == PointCloud(a, 0))
{
Match6(i,0) = PointCloud(a, 0);
Match6(i,1) = PointCloud(a, 1);
Match6(i,2) = PointCloud(a, 2);
Match6(i,3) = PointCloud(a, 3);
}
}
}
//d.b Using Function from Task 4 to calculate the correct plane parameter
//Match[i] where i <= 6, are all the Matrices that hold the correlating points.
//d.c Insert plane object into the vector of Plane
//d.d calculate and stuff
//part e, creating a new matrix and concatenate with the others.
//part f, output file
//WriteMatrixToFile(/*MatrixName*/, "OutputPointCloud.txt", 15);
//part g, convert Plane vector to Eigen Matrix
}