void Bundle2::loadGeometry_(H5::H5File& file) {
H5::Group geometryGroup = file.openGroup("/Geometry");
// Loading poses
H5::DataSet posesDataSet = geometryGroup.openDataSet("Poses");
double* posesData = (double*)malloc(frames_.size()*12*sizeof(double));
posesDataSet.read((void*)posesData, H5::PredType::NATIVE_DOUBLE, H5::DataSpace::ALL, H5::DataSpace::ALL);
posesDataSet.close();
size_t i = 0;
for(deque<Frame*>::iterator it = frames_.begin(); it != frames_.end(); ++it) {
Pose* pose = new Pose;
pose->sett(core::RealPoint3D<double>(posesData[i*12], posesData[i*12 + 1], posesData[i*12 + 2]));
core::Matrix<double> R(3, 3);
R[0][0] = posesData[i*12 + 3]; R[1][0] = posesData[i*12 + 4]; R[2][0] = posesData[i*12 + 5];
R[0][1] = posesData[i*12 + 6]; R[1][1] = posesData[i*12 + 7]; R[2][1] = posesData[i*12 + 8];
R[0][2] = posesData[i*12 + 9]; R[1][2] = posesData[i*12 + 10]; R[2][2] = posesData[i*12 + 11];
pose->setR(R);
pose->calcEulerAngles();
pose->setorientationSynchronWithAngles(true);
pose->setderivationsSynchronWithAngles(false);
(*it)->setpose(pose);
++i;
}
free((void*)posesData);
// Loading points
H5::DataSet pointsDataSet = geometryGroup.openDataSet("Points");
double* pointsData = (double*)malloc(tracks_.size()*3*sizeof(double));
pointsDataSet.read((void*)pointsData, H5::PredType::NATIVE_DOUBLE, H5::DataSpace::ALL, H5::DataSpace::ALL);
pointsDataSet.close();
i = 0;
for(deque<Track*>::iterator it = tracks_.begin(); it != tracks_.end(); it++) {
Point* point = new Point(core::RealPoint3D<double>(pointsData[i*3], pointsData[i*3 + 1], pointsData[i*3 + 2]));
(*it)->setpoint(point);
++i;
}
free((void*)pointsData);
// Loading inlier information
H5::DataSet inliersDataSet = geometryGroup.openDataSet("Inliers");
hvl_t* inliersData = (hvl_t*)malloc(frames_.size()*sizeof(hvl_t));
H5::VarLenType memType(&H5::PredType::NATIVE_UCHAR);
inliersDataSet.read((void*)inliersData, memType, H5::DataSpace::ALL, H5::DataSpace::ALL);
memType.close();
inliersDataSet.close();
i = 0;
for(deque<Frame*>::iterator it = frames_.begin(); it != frames_.end(); it++) {
unsigned char* inl = (unsigned char*)(inliersData[i].p);
size_t k = 0;
for(size_t j = 0; j < (*it)->size(); ++j) {
View& v = (**it)[j];
for(unsigned int cam = 0; cam < v.numCameras(); ++cam) {
if(v.inCamera(cam)) {
Ray ray;
if(inl[k]) ray.setinlier(true);
else ray.setinlier(false);
v.addRay(cam, ray);
++k;
}
}
}
++i;
}
for(size_t j = 0; j < frames_.size(); ++j) free(inliersData[j].p);
free((void*)inliersData);
// Loading curves if they exists
bool curvesFound = false;
const hsize_t maxObjs = geometryGroup.getNumObjs();
for(hsize_t obj = 0; obj < maxObjs; ++obj) {
string objName = geometryGroup.getObjnameByIdx(obj);
if(objName == string("Curves")) curvesFound = true;
}
if(curvesFound) {
H5::DataSet curvesDataSet = geometryGroup.openDataSet("Curves");
hsize_t curvesDim[1];
H5::DataSpace curvesDS = curvesDataSet.getSpace();
curvesDS.getSimpleExtentDims(curvesDim);
curvesDS.close();
hvl_t* curvesData = (hvl_t*)malloc(curvesDim[0]*sizeof(hvl_t));
H5::VarLenType memType(&H5::PredType::NATIVE_HSIZE);
curvesDataSet.read((void*)curvesData, memType, H5::DataSpace::ALL, H5::DataSpace::ALL);
memType.close();
curvesDataSet.close();
for(size_t c = 0; c < curvesDim[0]; ++c) {
const size_t cur_c = addCurve();
for(size_t p = 0; p < curvesData[c].len; ++p) {
curves_[cur_c].addPoint(((size_t*)(curvesData[c].p))[p]);
}
}
for(size_t i = 0; i < curvesDim[0]; ++i) free(curvesData[i].p);
free((void*)curvesData);
}
geometryGroup.close();
}
