|
6 | 6 | #include <geometry_msgs/PoseArray.h> |
7 | 7 | #include <random> |
8 | 8 | #include <ctime> |
9 | | -#include "PA_ceres_head.hpp" |
10 | 9 | using namespace std; |
11 | 10 |
|
12 | 11 | const double one_three = (1.0 / 3.0); |
@@ -101,134 +100,6 @@ void data_show(const vector<IMUST> &xBuf, vector<pcl::PointCloud<PointType>::Ptr |
101 | 100 | pub_pl_func(pl_send, pub_test); |
102 | 101 | } |
103 | 102 |
|
104 | | -class PlaneAdjustmentCeres |
105 | | -{ |
106 | | -public: |
107 | | - double ceres_iteration(vector<IMUST> &x_stats, vector<pcl::PointCloud<PointType>::Ptr> &plSurfs) |
108 | | - { |
109 | | - printf("Plane Adjustment (PA) is used.\n"); |
110 | | - |
111 | | - PLV(3) rot_params, pos_params, pla_params; |
112 | | - vector<PLM(4)*> plvecMat4s; |
113 | | - |
114 | | - int win_size = x_stats.size(); |
115 | | - for(int i=0; i<win_size; i++) |
116 | | - { |
117 | | - rot_params.push_back(Log(x_stats[i].R)); |
118 | | - pos_params.push_back(x_stats[i].p); |
119 | | - } |
120 | | - |
121 | | - for(pcl::PointCloud<PointType>::Ptr plPtr : plSurfs) |
122 | | - { |
123 | | - plvecMat4s.push_back(new PLM(4)(winSize)); |
124 | | - PLM(4) &plvecVoxel = *plvecMat4s.back(); |
125 | | - |
126 | | - for(Eigen::Matrix4d &mat : plvecVoxel) |
127 | | - mat.setZero(); |
128 | | - |
129 | | - PointCluster vf; |
130 | | - for(PointType &ap : plPtr->points) |
131 | | - { |
132 | | - int fn = ap.intensity; |
133 | | - Eigen::Vector4d pvec(ap.x, ap.y, ap.z, 1); |
134 | | - plvecVoxel[fn] += pvec * pvec.transpose(); |
135 | | - vf.push(x_stats[fn].R * pvec.head(3) + x_stats[fn].p); |
136 | | - } |
137 | | - |
138 | | - for(Eigen::Matrix4d &mat : plvecVoxel) |
139 | | - { |
140 | | - Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> saes(mat); |
141 | | - Eigen::Vector4d evalue = saes.eigenvalues(); |
142 | | - Eigen::Matrix4d mleft = saes.eigenvectors(); |
143 | | - Eigen::Matrix4d mvalue; mvalue.setZero(); |
144 | | - for(int i=0; i<4; i++) |
145 | | - { |
146 | | - if(evalue[i] > 0) |
147 | | - mvalue(i, i) = sqrt(evalue[i]); |
148 | | - } |
149 | | - mat = (mleft * mvalue).transpose(); |
150 | | - // mat = mat.llt().matrixL().transpose(); |
151 | | - } |
152 | | - |
153 | | - Eigen::Vector3d center = vf.v / vf.N; |
154 | | - Eigen::Matrix3d covMat = vf.P / vf.N - center * center.transpose(); |
155 | | - Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat); |
156 | | - Eigen::Vector3d pi = saes.eigenvectors().col(0); |
157 | | - double d = - pi.dot(center); |
158 | | - pla_params.push_back(d * pi); |
159 | | - } |
160 | | - |
161 | | - double t1 = ros::Time::now().toSec(); |
162 | | - |
163 | | - ceres::Problem problem; |
164 | | - for(int i=0; i<win_size; i++) |
165 | | - { |
166 | | - ceres::LocalParameterization *parametrization = new ParamSO3(); |
167 | | - problem.AddParameterBlock(rot_params[i].data(), 3, parametrization); |
168 | | - problem.AddParameterBlock(pos_params[i].data(), 3); |
169 | | - } |
170 | | - // problem.SetParameterBlockConstant(rot_params[0].data()); |
171 | | - // problem.SetParameterBlockConstant(pos_params[0].data()); |
172 | | - |
173 | | - for( int i= 0; i<plvecMat4s. size(); i++) |
174 | | - { |
175 | | - PLM(4) &plvecVoxel = *plvecMat4s[i]; |
176 | | - for(int j=0; j<win_size; j++) |
177 | | - { |
178 | | - PACeresFactor *f = new PACeresFactor(plvecVoxel[j]); |
179 | | - problem.AddResidualBlock(f, NULL, rot_params[j].data(), pos_params[j].data(), pla_params[i].data()); |
180 | | - } |
181 | | - } |
182 | | - |
183 | | - ceres::Solver::Options options; |
184 | | - // options.linear_solver_type = ceres::SPARSE_SCHUR; |
185 | | - options.linear_solver_type = ceres::DENSE_SCHUR; |
186 | | - options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; |
187 | | - options.max_num_iterations = 1000; |
188 | | - options.minimizer_progress_to_stdout = true; |
189 | | - |
190 | | - options.function_tolerance = 1e-10; |
191 | | - options.parameter_tolerance = 1e-10; |
192 | | - |
193 | | - options.use_inner_iterations = true; |
194 | | - ceres::ParameterBlockOrdering* ordering = new ceres::ParameterBlockOrdering; |
195 | | - for(int i=0; i<win_size; i++) |
196 | | - { |
197 | | - ordering->AddElementToGroup(rot_params[i].data(), 0); |
198 | | - ordering->AddElementToGroup(pos_params[i].data(), 1); |
199 | | - } |
200 | | - for( int i= 0; i<pla_params. size(); i++) |
201 | | - { |
202 | | - ordering->AddElementToGroup(pla_params[i].data(), 2); |
203 | | - } |
204 | | - options.inner_iteration_ordering.reset(ordering); |
205 | | - |
206 | | - ceres::Solver::Summary summary; |
207 | | - ceres::Solve(options, &problem, &summary); |
208 | | - // cout << summary.BriefReport() << endl; |
209 | | - // cout << summary.FullReport() << endl; |
210 | | - |
211 | | - for(int i=0; i<win_size; i++) |
212 | | - { |
213 | | - x_stats[i].R = Exp(rot_params[i]); |
214 | | - x_stats[i].p = pos_params[i]; |
215 | | - } |
216 | | - |
217 | | - for(int j=1; j<winSize; j++) |
218 | | - { |
219 | | - x_stats[j].p = x_stats[0].R.transpose() * (x_stats[j].p - x_stats[0].p); |
220 | | - x_stats[j].R = x_stats[0].R.transpose() * x_stats[j].R; |
221 | | - } |
222 | | - |
223 | | - x_stats[0].R.setIdentity(); |
224 | | - x_stats[0].p.setZero(); |
225 | | - |
226 | | - double t2 = ros::Time::now().toSec(); |
227 | | - return t2 - t1; |
228 | | - } |
229 | | - |
230 | | -}; |
231 | | - |
232 | 103 | class BALM2 |
233 | 104 | { |
234 | 105 | public: |
@@ -674,8 +545,6 @@ int main(int argc, char **argv) |
674 | 545 | printf("pstSize: %d\n", ptsSize); |
675 | 546 |
|
676 | 547 | int tseed = time(0); |
677 | | - // int tseed = 1662299903; |
678 | | - |
679 | 548 | default_random_engine e(tseed); |
680 | 549 | uniform_real_distribution<double> randNorml(-M_PI, M_PI); |
681 | 550 | uniform_real_distribution<double> randRange(-surf_range, surf_range); |
|
0 commit comments