int svlCameraGeometry::LoadCalibration(const std::string & filepath)
{
// Currently, only stereo calibration files created
// by the Camera Calibration Toolbox for Matlab are supported
// http://www.vision.caltech.edu/bouguetj/calib_doc/
Empty();
int success = SVL_OK;
double fc[2], cc[2], a, kc[5], om[3], T[3];
int pos;
char chstr[2048];
std::string str;
std::ifstream fin;
// reading whole file into string buffer
fin.exceptions(std::ios_base::eofbit | std::ios_base::failbit | std::ios_base::badbit);
try {
fin.open(filepath.c_str());
memset(chstr, 0, 2048);
pos = static_cast<int>(fin.read(chstr, 2047).gcount());
} catch (std::ifstream::failure e) {
}
fin.close();
str.assign(chstr);
std::istringstream strstrm(str);
// re-reading file and parsing values
strstrm.exceptions(std::ios_base::eofbit | std::ios_base::failbit | std::ios_base::badbit);
try {
///////////////////////////
// LEFT CAMERA INTRINSIC
// left focal length
pos = static_cast<int>(str.find("fc_left = [ "));
if (pos >= 0) {
strstrm.seekg(pos + 12);
strstrm >> fc[0] >> fc[1];
}
else success = SVL_FAIL;
// left principal point
pos = static_cast<int>(str.find("cc_left = [ "));
if (pos >= 0) {
strstrm.seekg(pos + 12);
strstrm >> cc[0] >> cc[1];
}
void CCDKSWindow::AddText(char *txt, bool wrap, int pos, int maxch)
{
if (maxch == -1) maxch = m_pSWindow->boxWidth-2;
if (wrap)
{
std::istringstream fullstrm(txt);
std::string line;
while (fullstrm)
{
if (!std::getline(fullstrm, line))
break;
if (line.length() > maxch)
{
while (line.length() > maxch)
{
std::string::size_type strpos = line.find_last_of(" \t\r\n", maxch-1);
if (strpos == std::string::npos)
strpos = maxch-1;
addCDKSwindow(m_pSWindow, const_cast<char *>(line.substr(0, strpos).c_str()), pos);
line.erase(0, strpos+1);
}
addCDKSwindow(m_pSWindow, const_cast<char *>(line.c_str()), pos);
}
else
addCDKSwindow(m_pSWindow, const_cast<char *>(line.c_str()), pos);
}
}
else
{
std::istringstream strstrm(txt);
std::string line;
while (strstrm)
{
if (std::getline(strstrm, line))
addCDKSwindow(m_pSWindow, const_cast<char *>(line.c_str()), pos);
}
}
}
void csv_64T_Process::Process()
{
src.open(T64);
if (!src.is_open())
{
throw runtime_error(ERR_FO + string(T64));
}
string line;
// Skipping the first lines
for (int i = 0; i < 3; i++)
{
getline(src, line);
}
bool readTopo = true;
vector<int> apos; // apostrophe
vector<string> type;
vector<int> breaks;
vector<int> dashes;
while (line.size() > 0 && readTopo)
{
int aposInd = line.find(APOSTROPHE) + ONE;
int breaksInd[2];
breaksInd[FIRST] = line.find(BREAKS) + ONE; // first occurrence
breaksInd[SECOND] = line.rfind(BREAKS) + ONE; // second occurrence (from the end)
int dash = boost::count(line, DASH) - 1; // -1 for the end of the line (doesn't count)
if (NOT_FOUND != breaksInd[FIRST]) // if found any |
{
if (NOT_FOUND == aposInd)
{
apos.push_back(ZERO);
type.push_back(boost::trim_copy(line.substr(breaksInd[SECOND] + 1))); // Pushing to the end of the vector the type.
breaks.push_back(breaksInd[SECOND]);
dashes.push_back(dash);
}
else
{
apos.push_back(aposInd);
type.push_back(boost::trim_copy(line.substr(breaksInd[FIRST] + 1)));
breaks.push_back(breaksInd[FIRST]);
dashes.push_back(dash);
}
}
else
{
readTopo = false;
}
getline(src, line);
}
vector<int> origApos = apos; // (geresh)
vector<int> origDashes = dashes;
dashes.clear();
vector<int> q = { 0 }; // check if int
vector<int> parent = { 0 };
vector<int> child_seg = { 0 };
int ind = 0;
vector<int> type_inds;
//type_inds.resize(type.size());
int type_ind;
vector<string> types;
types.resize(type.size());
int node_break;
if (apos.size() != ZERO)
{
while (q.size() != ZERO)
{
type_ind = q[ZERO];
vector<int> temp = { type_ind };
if (HocHelpFunc::Find(HocHelpFunc::VectorComparison(temp, type_inds)).size() == ZERO)
{
type_inds.resize(ind + ONE);
type_inds[ind] = type_ind;
types[ind] = type[type_ind];
dashes.resize(ind + ONE);
dashes[ind] = origDashes[type_ind];
node_break = breaks[type_ind];
if (type_ind + ONE == apos.size() || apos[type_ind] >= apos[type_ind + ONE])
{
ind++;
q = HocHelpFunc::SubVector(q, ONE, q.size() - ONE);
continue;
}
vector<int> end_of_tree = HocHelpFunc::Find(HocHelpFunc::VectorSmallerComparison(
HocHelpFunc::SubVector(origApos, type_ind, origApos.size() - ONE),
HocHelpFunc::SubVector(origApos, type_ind, type_ind)),
1, true);
if (end_of_tree.size() != ZERO)
{
for (size_t i = 0; i < end_of_tree.size(); i++)
{
end_of_tree[i] -= ONE;
}
}
if (end_of_tree.size() == ZERO || origApos[type_ind] == ZERO)
{
end_of_tree.push_back(apos.size());
}
else
{
// Not sure what is here.. TODO
}
vector<int> childs_inds = HocHelpFunc::Find(HocHelpFunc::VectorSmallerEqualsComparison(
HocHelpFunc::SubVector(apos, type_ind + ONE, end_of_tree[0] - ONE), // TODO not sure end_of_tree[0] is the right translate from matlab
node_break + ONE));
for (size_t i = 0; i < childs_inds.size(); i++)
{
if (ZERO == type_ind)
{
childs_inds[i] += (type_ind + ONE);
}
else
{
childs_inds[i] += type_ind;
}
}
vector<int> zero_child = HocHelpFunc::Find(
HocHelpFunc::VectorComparison(
HocHelpFunc::SubVector(apos, childs_inds), ZERO));
if (zero_child.size() != ZERO)
{
//hilds_inds[] TODO need to make some function that matches.
}
vector<int> adopted;
for (size_t i = 0; i < childs_inds.size(); i++)
{
int cur_ind = childs_inds[i];
vector<int> true_parent = HocHelpFunc::Find(HocHelpFunc::VectorSmallerComparison(
HocHelpFunc::SubVector(apos, type_ind, cur_ind - ONE), // cur_ind - maby -2. TODO
apos[cur_ind]));
temp.clear();
for (size_t j = 0; j < true_parent.size(); j++)
{
if (type_ind == ZERO)
{
temp.push_back(true_parent[j] + ONE);
}
else
{
temp.push_back(true_parent[j] + type_ind);
}
}
true_parent = HocHelpFunc::Find(HocHelpFunc::VectorSmallerComparison(
apos[cur_ind], HocHelpFunc::SubVector(breaks, temp)));
if (HocHelpFunc::Find(HocHelpFunc::VectorSmallerComparison(ONE, true_parent)).size() != ZERO)
{
adopted.push_back(i);
}
// stopped at line 85 in matlab
}
if (adopted.size() != ZERO)
{
childs_inds.erase(childs_inds.begin() + adopted[ZERO]);
}
if (childs_inds.size() == ZERO && ind == ZERO)
{
childs_inds = HocHelpFunc::Find(HocHelpFunc::VectorComparison(apos, node_break));
}
vector<int> cur_seg;
for (size_t i = 0; i < childs_inds.size(); i++)
{
int cur_child = childs_inds[i];
int child_start = apos[cur_child];
int parent_start;
if (type_ind == ZERO)
{
parent_start = 1;
}
else
{
parent_start = apos[type_ind];
}
cur_seg.push_back(child_start - parent_start);
}
child_seg = HocHelpFunc::ConnectVectors(child_seg, HocHelpFunc::FlipLr(cur_seg));
if (childs_inds.size() != ZERO)
{
q = HocHelpFunc::ConnectVectors(q, HocHelpFunc::FlipLr(childs_inds));
parent = HocHelpFunc::ConnectVectors(parent, (HocHelpFunc::HomoVecNum(childs_inds.size(), ONE * ind + ONE))); // I don't see any reason for this line in the matlab code..
}
ind++;
}
q = HocHelpFunc::SubVector(q, 1, q.size() - ONE);
}
}
else
{
types = type;
}
type = types;
getline(src, line);
getline(src, line);
comp_params.resize(type.size());
while (line.size() != ZERO)
{
string cur_type = boost::trim_copy(line);
int comp_ind = GetCompIndex(type, cur_type);
getline(src, line);
// Getting the floating numbers in the line.
// ATTENTION!! because of the VS there is a small rounding with the numbers - so they are not exactly as they are in the 64T file.
vector<float> dbl_vec;
stringstream strstrm(line);
float i;
while (strstrm >> i)
{
dbl_vec.push_back(i);
if (strstrm.peek() == C_COMMA)
strstrm.ignore();
}
comp_params[comp_ind] = (dbl_vec);
getline(src, line);
}
src.close();
}