51 assert(translateA[3] == 1.0);
52 assert(translateB[3] == 1.0);
55 double dist = (translateB - translateA).
norm();
58 Rotationd bTransposed = rotateB.transpose();
61 double tmp = (r.trace() / 2.0) - 0.5;
73 double angle = std::acos(tmp);
75 if(m_options->verbose)
77 std::cout <<
"PoseDiff: " << dist <<
" ; AngleDiff: " << angle << std::endl;
80 return (angle < m_options->diffAngle && dist < m_options->diffPosition);
86 Eigen::Matrix<double, 3, 3> tmp_mat(inputMatrix4x4->block<3,3>(0,0));
91 double cosA = cos(angle);
92 double sinA = sin(angle);
95 Eigen::Matrix<double, 3, 3> mult_mat;
96 mult_mat << v(0)*v(0)*(1.0-cosA)+cosA, v(0)*v(1)*(1.0-cosA)-v(2)*sinA, v(0)*v(2)*(1.0-cosA)+v(1)*sinA,
97 v(1)*v(0)*(1.0-cosA)+v(2)*sinA, v(1)*v(1)*(1.0-cosA)+cosA, v(1)*v(2)*(1.0-cosA)-v(0)*sinA,
98 v(2)*v(0)*(1.0-cosA)-v(1)*sinA, v(2)*v(1)*(1.0-cosA)+v(0)*sinA, v(2)*v(2)*(1.0-cosA)+cosA;
99 tmp_mat = tmp_mat * mult_mat;
102 inputMatrix4x4[0](0,0) = tmp_mat(0,0);
103 inputMatrix4x4[0](0,1) = tmp_mat(0,1);
104 inputMatrix4x4[0](0,2) = tmp_mat(0,2);
105 inputMatrix4x4[0](1,0) = tmp_mat(1,0);
106 inputMatrix4x4[0](1,1) = tmp_mat(1,1);
107 inputMatrix4x4[0](1,2) = tmp_mat(1,2);
108 inputMatrix4x4[0](2,0) = tmp_mat(2,0);
109 inputMatrix4x4[0](2,1) = tmp_mat(2,1);
110 inputMatrix4x4[0](2,2) = tmp_mat(2,2);
124 for (
size_t i = 0; i < m_scans->project->positions.size(); i++)
126 if(m_scans->project->positions.at(i)->scans.size())
129 rotateAroundYAxis(&(m_scans->project->positions[i]->scans[0]->poseEstimation), m_options->rotate_angle *
M_PI / 180);
132 ScanPtr scptr = std::make_shared<Scan>(*(m_scans->project->positions[i]->scans[0]));
137 if (m_options->verbose)
139 cout <<
"start SLAMAlign registration" << endl;
145 if (m_options->verbose)
147 cout <<
"end SLAMAlign registration" << endl;
151 bool all_values_new =
true;
152 for (
int i = 0; i < m_scans->project->positions.size(); i++)
157 if (( !m_scans->changed.at(i)) &&
158 !isToleratedDifference(
159 m_scans->project->positions.at(i)->scans[0]->registration,
160 align.
scan(i)->pose()))
162 m_scans->changed.at(i) =
true;
163 cout <<
"New Values"<< endl;
168 all_values_new =
false;
171 cout <<
"First registration done" << endl;
176 cout <<
"no new registration" << endl;
180 cout <<
"start new registration with some fix poses" << endl;
183 align =
SLAMAlign(*m_options, m_scans->changed);
185 for (
size_t i = 0; i < m_scans->project->positions.size(); i++)
187 if(m_scans->project->positions.at(i)->scans.size())
189 ScanPtr scptr = std::make_shared<Scan>(*(m_scans->project->positions[i]->scans[0]));
199 for (
int i = 0; i < m_scans->project->positions.size(); i++)
203 if (m_scans->changed.at(i) || all_values_new)
205 posPtr->scans[0]->registration = align.
scan(i)->pose();
206 posPtr->registration = align.
scan(i)->pose();
207 cout <<
"Pose Scan Nummer " << i << endl << posPtr->scans[0]->registration << endl;