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); }