OSG::Line VRPyBase::PyToLine(PyObject *li) {
if (li == 0) return OSG::Line();
vector<PyObject*> lis = pyListToVector(li);
if (lis.size() != 6) return OSG::Line();
float r[6];
for (int i=0; i<6; i++) r[i] = PyFloat_AsDouble(lis[i]);
return OSG::Line(OSG::Pnt3f(r[3],r[4],r[5]), OSG::Vec3f(r[0],r[1],r[2]));
}
OSG::Matrix4d VRPyBase::parseMatrixList(PyObject *li) {
if (li == 0) return OSG::Matrix4d();
vector<PyObject*> lis = pyListToVector(li);
if (lis.size() != 16) return OSG::Matrix4d();
float d[16];
for (int i=0; i<16; i++) d[i] = PyFloat_AsDouble(lis[i]);
return OSG::Matrix4d(d[0], d[1], d[2], d[3], d[4], d[5],d[6],d[7],d[8], d[9],d[10], d[11], d[12], d[13],d[14], d[15]);
}
OSG::Vec2i VRPyBase::parseVec2iList(PyObject *li) {
if (li == 0) return OSG::Vec2i();
vector<PyObject*> lis = pyListToVector(li);
if (lis.size() != 2) return OSG::Vec2i();
int x,y;
x = PyInt_AsLong(lis[0]);
y = PyInt_AsLong(lis[1]);
return OSG::Vec2i(x,y);
}
OSG::Vec3f VRPyBase::parseVec3fList(PyObject *li) {
vector<PyObject*> lis = pyListToVector(li);
if (lis.size() != 3) return OSG::Vec3f(0,0,0);
float x,y,z;
x = PyFloat_AsDouble(lis[0]);
y = PyFloat_AsDouble(lis[1]);
z = PyFloat_AsDouble(lis[2]);
return OSG::Vec3f(x,y,z);
}
OSG::Vec2d VRPyBase::parseVec2dList(PyObject *li) {
if (li == 0) return OSG::Vec2d();
if (VRPyVec2f::check(li)) return ((VRPyVec2f*)li)->v;
vector<PyObject*> lis = pyListToVector(li);
if (lis.size() != 2) return OSG::Vec2d();
float x,y;
x = PyFloat_AsDouble(lis[0]);
y = PyFloat_AsDouble(lis[1]);
return OSG::Vec2d(x,y);
}
PyObject* VRPyMaterial::setTexture(VRPyMaterial* self, PyObject* args) {
if (self->objPtr == 0) { PyErr_SetString(err, "VRPyMaterial::setTexture, C obj is invalid"); return NULL; }
int aN = pySize(args);
if (aN == 1) {
PyObject* o = parseObject(args);
if (PyString_Check(o)) self->objPtr->setTexture( PyString_AsString(o) ); // load a file
else if (VRPyImage::check(o)) {
VRPyImage* img = (VRPyImage*)o;
self->objPtr->setTexture( img->objPtr, 0 );
}
}
if (aN > 1) {
PyObject *data, *dims; int doFl;
if (! PyArg_ParseTuple(args, "OOi", &data, &dims, &doFl)) return NULL;
if (pySize(data) == 0) Py_RETURN_TRUE;
vector<PyObject*> _data = pyListToVector(data);
int dN = _data.size();
int vN = pySize(_data[0]);
if (doFl) {
vector<float> buf(vN*dN, 0);
for (int i=0; i<dN; i++) {
PyObject* dObj = _data[i];
vector<PyObject*> vec = pyListToVector(dObj);
for (int j=0; j<vN; j++) buf[i*vN+j] = PyFloat_AsDouble(vec[j]);
}
self->objPtr->setTexture( (char*)&buf[0], vN, parseVec3iList(dims), true );
} else {
vector<char> buf(vN*dN, 0);
for (int i=0; i<dN; i++) {
vector<PyObject*> vec = pyListToVector(_data[i]);
for (int j=0; j<vN; j++) buf[i*vN+j] = PyInt_AsLong(vec[j]);
}
self->objPtr->setTexture( &buf[0], vN, parseVec3iList(dims), false );
}
}
Py_RETURN_TRUE;
}
OSG::Vec4i VRPyBase::parseVec4iList(PyObject *li) {
if (li == 0) return OSG::Vec4i();
vector<PyObject*> lis = pyListToVector(li);
if (lis.size() != 4) return OSG::Vec4i();
int x,y,z,w;
x = PyInt_AsLong(lis[0]);
y = PyInt_AsLong(lis[1]);
z = PyInt_AsLong(lis[2]);
w = PyInt_AsLong(lis[3]);
return OSG::Vec4i(x,y,z,w);
}
OSG::Vec4d VRPyBase::parseVec4dList(PyObject *li) {
if (li == 0) return OSG::Vec4d();
vector<PyObject*> lis = pyListToVector(li);
if (lis.size() != 4) return OSG::Vec4d();
float x,y,z,w;
x = PyFloat_AsDouble(lis[0]);
y = PyFloat_AsDouble(lis[1]);
z = PyFloat_AsDouble(lis[2]);
w = PyFloat_AsDouble(lis[3]);
return OSG::Vec4d(x,y,z,w);
}
PyObject* VRPyMechanism::addChain(VRPyMechanism* self, PyObject* args) {
if (self->obj == 0) { PyErr_SetString(err, "VRPyMechanism::addChain - Object is invalid"); return NULL; }
float w; PyObject *l, *dirs;
if (! PyArg_ParseTuple(args, "fOO", &w, &l, &dirs)) return NULL;
vector<PyObject*> objs = pyListToVector(l);
vector<OSG::VRGeometry*> geos;
for (auto o : objs) {
VRPyGeometry* g = (VRPyGeometry*)o;
geos.push_back( g->obj );
}
return VRPyTypeCaster::cast( self->obj->addChain(w, geos, PyString_AsString(dirs) ) );
}
void Parameters::initialize()
{
if (initialized)
return;
Py_Initialize();
py::object pycfg = py::import("pipeline_config");
// Importing parameters from config file
dset_dir = py::extract<std::string>(pycfg.attr("DSET_DIR"));
dset_avi = py::extract<std::string>(pycfg.attr("DSET_AVI"));
start = py::extract<int>(pycfg.attr("EXPORT_START"));
step = py::extract<int>(pycfg.attr("EXPORT_STEP"));
count = py::extract<int>(pycfg.attr("EXPORT_NUM"));
end = start + step * count;
h5_dir = py::extract<std::string>(pycfg.attr("POINTS_H5_DIR"));
pcd_dir = py::extract<std::string>(pycfg.attr("PCD_DIR"));
pcd_downsampled_dir = py::extract<std::string>(pycfg.attr("PCD_DOWNSAMPLED_DIR"));
color_dir = py::extract<std::string>(pycfg.attr("COLOR_DIR"));
color_clouds_dir = py::extract<std::string>(pycfg.attr("COLOR_CLOUDS_DIR"));
params_file = py::extract<std::string>(pycfg.attr("PARAMS_H5_FILE"));
cam_ind = py::extract<int>(pycfg.attr("CAMERA")) - 1;
lidar_project_min_dist = py::extract<float>(pycfg.attr("LIDAR_PROJECT_MIN_DIST"));
py::list l = py::extract<py::list>(pycfg.attr("ICP_COORD_WEIGHTS"));
for (int j = 0; j < py::len(l); j++)
icp_coord_weights.push_back(py::extract<float>(l[j]));
icp_tol = py::extract<float>(pycfg.attr("ICP_TOL"));
icp_min_intensity = py::extract<float>(pycfg.attr("ICP_MIN_INTENSITY"));
map_color_window = py::extract<int>(pycfg.attr("MAP_COLOR_WINDOW"));
cloud_max_store = py::extract<int>(pycfg.attr("CLOUD_MAX_STORE"));
handle_occlusions = py::extract<bool>(pycfg.attr("HANDLE_OCCLUSIONS"));
octree_res = py::extract<float>(pycfg.attr("OCTOMAP_RES"));
color_octree_res = py::extract<float>(pycfg.attr("COLOR_OCTOMAP_RES"));
prob_hit = py::extract<float>(pycfg.attr("PROB_HIT"));
prob_miss = py::extract<float>(pycfg.attr("PROB_MISS"));
occupancy_thres = py::extract<float>(pycfg.attr("OCCUPANCY_THRES"));
clamping_thres_max = py::extract<float>(pycfg.attr("CLAMPING_THRES_MAX"));
clamping_thres_min = py::extract<float>(pycfg.attr("CLAMPING_THRES_MIN"));
raycast_tol = py::extract<float>(pycfg.attr("RAYCAST_TOL"));
cast_octomap_single = py::extract<bool>(pycfg.attr("CAST_OCTOMAP_SINGLE"));
cast_once = py::extract<bool>(pycfg.attr("CAST_ONCE"));
octomap_file = py::extract<std::string>(pycfg.attr("OCTOMAP_FILE"));
centered_octomap_file = py::extract<std::string>(pycfg.attr("CENTERED_OCTOMAP_FILE"));
color_octomap_file = py::extract<std::string>(pycfg.attr("COLOR_OCTOMAP_FILE"));
centered_color_octomap_file = py::extract<std::string>(pycfg.attr("CENTERED_COLOR_OCTOMAP_FILE"));
octomap_h5_file = py::extract<std::string>(pycfg.attr("OCTOMAP_H5_FILE"));
color_octomap_h5_file = py::extract<std::string>(pycfg.attr("COLOR_OCTOMAP_H5_FILE"));
pyListToVector(pycfg.attr("OCTOMAP_SINGLE_FILES"), octomap_single_files);
cluster_tol = py::extract<float>(pycfg.attr("CLUSTER_TOL"));
min_cluster_size = py::extract<int>(pycfg.attr("MIN_CLUSTER_SIZE"));
max_cluster_size = py::extract<int>(pycfg.attr("MAX_CLUSTER_SIZE"));
// Lidar detection parameters
pyListToVector(pycfg.attr("FILTER_RANGE"), filter_range);
pyListToVector(pycfg.attr("G"), g);
inlier_dist_thresh = py::extract<float>(pycfg.attr("INLIER_DIST_THRESH"));
highest_plane = py::extract<bool>(pycfg.attr("HIGHEST_PLANE"));
plane_ransac_max_iters = py::extract<int>(pycfg.attr("PLANE_RANSAC_MAX_ITERS"));
normal_eps_angle = py::extract<float>(pycfg.attr("NORMAL_EPS_ANGLE"));
min_plane_cloud_size = py::extract<int>(pycfg.attr("MIN_PLANE_CLOUD_SIZE"));
min_pts_above_hull = py::extract<int>(pycfg.attr("MIN_PTS_ABOVE_HULL"));
pyListToVector(pycfg.attr("PLANE_Z_THRESH"), plane_z_thresh);
obj_cluster_tol = py::extract<float>(pycfg.attr("OBJ_CLUSTER_TOL"));
obj_min_cluster_size = py::extract<int>(pycfg.attr("OBJ_MIN_CLUSTER_SIZE"));
obj_max_cluster_size = py::extract<int>(pycfg.attr("OBJ_MAX_CLUSTER_SIZE"));
// Loading calibration parameters
H5::H5File params_h5f(params_file, H5F_ACC_RDONLY);
MatrixXfRowMajor row_major;
load_hdf_dataset(params_h5f, "/lidar/T_from_l_to_i", row_major, H5::PredType::NATIVE_FLOAT);
Eigen::Matrix4f T_from_l_to_i(row_major);
T_from_i_to_l = T_from_l_to_i.inverse();
trans_from_i_to_l = Eigen::Matrix4f::Identity();
trans_from_i_to_l.block(0, 3, 3, 1) = T_from_i_to_l.block(0, 3, 3, 1);
trans_from_l_to_c = Eigen::Matrix4f::Identity();
std::string s = (boost::format("/cam/%1%/displacement_from_l_to_c_in_lidar_frame") % cam_ind).str();
load_hdf_dataset(params_h5f, s, row_major, H5::PredType::NATIVE_FLOAT);
Eigen::Vector3f displacement_from_l_to_c_in_lidar_frame(row_major);
trans_from_l_to_c.block(0, 3, 3, 1) = displacement_from_l_to_c_in_lidar_frame;
rot_to_c_from_l = Eigen::Matrix4f::Identity();
s = (boost::format("/cam/%1%/R_to_c_from_l_in_camera_frame") % cam_ind).str();
load_hdf_dataset(params_h5f, s, row_major, H5::PredType::NATIVE_FLOAT);
Eigen::Matrix3f R_to_c_from_l_in_camera_frame(row_major);
// FIXME Too much hard-coding
Eigen::Matrix3f swap;
swap << 0.0, -1.0, 0.0,
0.0, 0.0, -1.0,
1.0, 0.0, 0.0;
rot_to_c_from_l.block(0, 0, 3, 3) = R_to_c_from_l_in_camera_frame * swap;
s = (boost::format("/cam/%1%/KK") % cam_ind).str();
load_hdf_dataset(params_h5f, s, row_major, H5::PredType::NATIVE_FLOAT);
Eigen::Matrix3f K(row_major);
intrinsics = K;
s = (boost::format("/cam/%1%/distort") % cam_ind).str();
load_hdf_dataset(params_h5f, s, row_major, H5::PredType::NATIVE_FLOAT);
Eigen::VectorXf D(row_major);
distortions = D;
params_h5f.close();
initialized = true;
}
vector<PyObject*> VRPyBase::parseList(PyObject *args) {
return pyListToVector( parseObject(args) );
}
vector<PyObject*> VRPyBase::parseList(PyObject *args) {
PyObject* v;
if (! PyArg_ParseTuple(args, "O", &v)) return vector<PyObject*>();
return pyListToVector(v);
}
