33 rightCameraPose = rCPose;
34 cameraPoseOnRobot = cPORobot;
38 const std::string& filename)
40 std::ofstream file(filename);
43 vector<TStereoImageFeatures>::iterator it;
44 for (it = theFeatures.begin(); it != theFeatures.end(); ++it)
46 "%u %.2f %.2f %.2f %.2f\n", it->ID, it->pixels.first.x,
47 it->pixels.first.y, it->pixels.second.x, it->pixels.second.y);
60 out << rightCameraPose << cameraPoseOnRobot;
61 out << (uint32_t)theFeatures.size();
63 for (
const auto& theFeature : theFeatures)
65 out << theFeature.pixels.first.x << theFeature.pixels.first.y;
66 out << theFeature.pixels.second.x << theFeature.pixels.second.y;
67 out << (uint32_t)theFeature.ID;
69 out << sensorLabel << timestamp;
82 in >> rightCameraPose >> cameraPoseOnRobot;
84 theFeatures.resize(nL);
85 for (
auto& theFeature : theFeatures)
87 in >> theFeature.pixels.first.x >> theFeature.pixels.first.y;
88 in >> theFeature.pixels.second.x >> theFeature.pixels.second.y;
90 theFeature.ID = (
unsigned int)nR;
92 in >> sensorLabel >> timestamp;
101 std::ostream& o)
const
105 o <<
"Homogeneous matrix for the sensor's 3D pose, relative to robot "
108 << cameraPoseOnRobot << endl;
110 o <<
"Homogeneous matrix for the RIGHT camera's 3D pose, relative to LEFT "
111 "camera reference system:\n";
113 << rightCameraPose << endl;
115 o <<
"Intrinsic parameters matrix for the LEFT camera:" << endl;
119 o <<
"Distortion parameters vector for the LEFT camera:" << endl <<
"[ ";
120 for (
unsigned int i = 0; i < 5; ++i) o << cameraLeft.dist[i] <<
" ";
123 o <<
"Intrinsic parameters matrix for the RIGHT camera:" << endl;
124 aux = cameraRight.intrinsicParams;
127 o <<
"Distortion parameters vector for the RIGHT camera:" << endl <<
"[ ";
128 for (
unsigned int i = 0; i < 5; ++i) o << cameraRight.dist[i] <<
" ";
133 " Image size: %ux%u pixels\n", (
unsigned int)cameraLeft.ncols,
134 (
unsigned int)cameraLeft.nrows);
137 " Number of features in images: %u\n",
138 (
unsigned int)theFeatures.size());