58 const size_t cur_iter,
const double cur_total_sq_error,
59 const size_t max_iters,
64 const double avr_err =
65 std::sqrt(cur_total_sq_error / input_observations.size());
67 if ((cur_iter % 10) == 0)
69 cout <<
"[PROGRESS] Iter: " << cur_iter
70 <<
" avrg err in px: " << avr_err << endl;
82 cout <<
"Optimizing " << allObs.size() <<
" feature observations.\n";
86 extra_params[
"max_iterations"] = 2000;
89 extra_params[
"robust_kernel"] = 0;
90 extra_params[
"kernel_param"] = 5.0;
91 extra_params[
"profiler"] = 1;
94 allObs, camera_params, frame_poses, landmark_points, extra_params,
100 const TFramePosesVec& poses,
const double len,
const double lineWidth);
114 <<
argv[0] <<
" --help -> Shows this help\n"
115 <<
argv[0] <<
" -> Simulation\n"
117 <<
" <feats.txt> <cam_model.cfg> -> Data in MRPT format\n"
119 <<
" <cams.txt> <points.cfg> <calib.txt> -> SBA format\n";
133 landmark_points_noisy;
142 camera_params.
ncols = 800;
143 camera_params.
nrows = 600;
144 camera_params.
fx(400);
145 camera_params.
fy(400);
146 camera_params.
cx(400);
147 camera_params.
cy(300);
151 const size_t nPts = 100;
154 const double L2 = 10;
155 const double L3 = 10;
156 const double max_camera_dist = L1 * 4;
158 const double cameraPathLen = L1 * 1.2;
162 const double STD_PX_ERROR = 0.10;
164 const double STD_PX_ERROR_OUTLIER = 5;
165 const double PROBABILITY_OUTLIERS = 0;
167 const double STD_PT3D = 0.10;
168 const double STD_CAM_XYZ = 0.05;
169 const double STD_CAM_ANG = 5.0_deg;
171 landmark_points_real.resize(nPts);
172 for (
size_t i = 0; i < nPts; i++)
174 landmark_points_real[i].x = rg.drawUniform(-L1, L1);
175 landmark_points_real[i].y = rg.drawUniform(-L2, L2);
176 landmark_points_real[i].z = rg.drawUniform(-L3, L3);
180 const double camPosesSteps = 2 * cameraPathLen / 20;
181 frame_poses_real.clear();
183 for (
double x = -cameraPathLen; x < cameraPathLen;
191 30.0_deg * x / cameraPathLen;
196 frame_poses_real.push_back(
201 size_t numOutliers = 0;
203 map<TCameraPoseID, size_t> numViewedFrom;
204 for (
size_t i = 0; i < frame_poses_real.size();
208 for (
size_t j = 0; j < landmark_points_real.size(); j++)
213 camera_params, frame_poses_real[i],
214 landmark_points_real[j]);
216 const bool is_outlier =
217 (rg.drawUniform(0.0, 1.0) < PROBABILITY_OUTLIERS);
218 px.
x += rg.drawGaussian1D(
219 0, is_outlier ? STD_PX_ERROR_OUTLIER : STD_PX_ERROR);
220 px.
y += rg.drawGaussian1D(
221 0, is_outlier ? STD_PX_ERROR_OUTLIER : STD_PX_ERROR);
224 if (px.
x < 0 || px.
y < 0 || px.
x > camera_params.ncols ||
225 px.
y > camera_params.nrows)
230 TPoint3D(frame_poses_real[i].asTPose()),
231 landmark_points_real[j]);
232 if (dist > max_camera_dist)
continue;
235 if (is_outlier) numOutliers++;
240 cout <<
"Simulated: " << allObs.size()
241 <<
" observations (of which: " << numOutliers
242 <<
" are outliers).\n";
244 ASSERT_EQUAL_(numViewedFrom.size(), frame_poses_real.size());
248 std::map<TCameraPoseID, TCameraPoseID> old2new_camIDs;
249 std::map<TLandmarkID, TLandmarkID> old2new_lmIDs;
250 allObs2.
compressIDs(&old2new_camIDs, &old2new_lmIDs);
252 ASSERT_EQUAL_(old2new_camIDs.size(), frame_poses_real.size());
254 old2new_lmIDs.size(), landmark_points_real.size());
258 frame_poses_noisy = frame_poses_real;
259 landmark_points_noisy = landmark_points_real;
260 for (
size_t i = 0; i < landmark_points_noisy.size(); i++)
261 landmark_points_noisy[i] +=
TPoint3D(
262 rg.drawGaussian1D(0, STD_PT3D),
263 rg.drawGaussian1D(0, STD_PT3D),
264 rg.drawGaussian1D(0, STD_PT3D));
266 for (
size_t i = 1; i < frame_poses_noisy.size();
269 CPose3D bef = frame_poses_noisy[i];
271 frame_poses_noisy[i].x() +
272 rg.drawGaussian1D(0, STD_CAM_XYZ),
273 frame_poses_noisy[i].y() +
274 rg.drawGaussian1D(0, STD_CAM_XYZ),
275 frame_poses_noisy[i].z() +
276 rg.drawGaussian1D(0, STD_CAM_XYZ),
277 frame_poses_noisy[i].yaw() +
278 rg.drawGaussian1D(0, STD_CAM_ANG),
279 frame_poses_noisy[i].pitch() +
280 rg.drawGaussian1D(0, STD_CAM_ANG),
281 frame_poses_noisy[i].roll() +
282 rg.drawGaussian1D(0, STD_CAM_ANG));
286 frame_poses = frame_poses_noisy;
287 landmark_points = landmark_points_noisy;
290 vector<std::array<double,2> > resids;
292 cout <<
"Initial avr error in px: " << std::sqrt(initial_total_sq_err/allObs.size()) << endl;
297 camera_params, allObs, frame_poses, landmark_points);
300 double landmarks_total_sq_err = 0;
301 for (
size_t i = 0; i < landmark_points.size(); i++)
302 landmarks_total_sq_err +=
square(
303 landmark_points_real[i].distanceTo(landmark_points[i]));
305 double cam_point_total_sq_err = 0;
306 for (
size_t i = 0; i < frame_poses.size(); i++)
307 cam_point_total_sq_err +=
308 square(frame_poses[i].distanceTo(frame_poses_real[i]));
310 cout <<
"RMSE of recovered landmark positions: "
311 << std::sqrt(landmarks_total_sq_err / landmark_points.size())
313 cout <<
"RMSE of recovered camera positions: "
314 << std::sqrt(cam_point_total_sq_err / frame_poses.size())
323 const string feats_fil = string(
argv[1]);
324 const string cam_fil = string(
argv[2]);
326 cout <<
"Loading observations from: " << feats_fil <<
"...";
335 cout <<
"Loading camera params from: " << cam_fil;
337 camera_params.loadFromConfigFile(
"CAMERA", cfgCam);
340 cout <<
"Initial gross estimate...";
342 allObs, camera_params, frame_poses, landmark_points);
349 const string cam_frames_fil = string(
argv[1]);
350 const string obs_fil = string(
argv[2]);
351 const string calib_fil = string(
argv[3]);
354 cout <<
"Loading initial camera frames from: "
355 << cam_frames_fil <<
"...";
361 std::istringstream ss;
362 while (fil.getNextLine(ss))
365 ss >> q[0] >> q[1] >> q[2] >> q[3] >> t[0] >> t[1] >>
370 q[0], q[1], q[2], q[3]));
372 frame_poses.push_back(
CPose3D(p));
375 cout <<
"Done. " << frame_poses.size()
376 <<
" cam frames loaded\n";
378 frame_poses_noisy = frame_poses;
383 cout <<
"Loading observations & feature 3D points from: "
388 landmark_points.clear();
391 std::istringstream ss;
392 while (fil.getNextLine(ss))
397 ss >> t[0] >> t[1] >> t[2] >> N;
399 const TLandmarkID feat_id = landmark_points.size();
400 const TPoint3D pt(t[0], t[1], t[2]);
401 landmark_points.push_back(pt);
404 for (
size_t i = 0; i < N; i++)
408 ss >> frame_id >> px.
x >> px.
y;
417 cout <<
"Done. " << landmark_points.size() <<
" points, "
418 << allObs.size() <<
" observations read.\n";
420 landmark_points_real = landmark_points;
430 camera_params.fx(cam_pars(0, 0));
431 camera_params.fy(cam_pars(1, 1));
432 camera_params.cx(cam_pars(0, 2));
433 camera_params.cy(cam_pars(1, 2));
435 cout <<
"camera calib:\n" << camera_params.dumpAsText() << endl;
443 camera_params, allObs, frame_poses, landmark_points);
453 auto obj = CGridPlaneXY::Create(-200, 200, -200, 200, 0, 5);
454 obj->setColor(0.7, 0.7, 0.7);
458 if (!landmark_points_real.empty())
460 auto obj = CPointCloud::Create();
461 obj->setPointSize(2);
462 obj->setColor(0, 0, 0);
463 obj->loadFromPointsList(landmark_points_real);
467 if (!landmark_points_noisy.empty())
469 auto obj = CPointCloud::Create();
470 obj->setPointSize(4);
471 obj->setColor(0.7, 0.2, 0.2, 0);
472 obj->loadFromPointsList(landmark_points_noisy);
478 auto obj = CPointCloud::Create();
479 obj->setPointSize(3);
480 obj->setColor(0, 0, 1, 1.0);
481 obj->loadFromPointsList(landmark_points);
491 win.setCameraZoom(100);
493 win.unlockAccess3DScene();
498 "Avr log-error with iterations", 500, 200);
503 cout <<
"Close the 3D window or press a key to exit.\n";
508 catch (
const std::exception& e)
510 std::cout <<
"MRPT exception caught: " << e.what() << std::endl;
515 printf(
"Untyped exception!!");
521 const TFramePosesVec& poses,
const double len,
const double lineWidth)
525 for (
size_t i = 0; i < poses.size(); i++)
534 corner->setName(
format(
"%u", (
unsigned int)i));
535 corner->enableShowName();