virtual void render(CameraInfo const &cam, int pass) { Matrix m; cam.getModelView(this, &m); glLoadTransposeMatrixf((float *)m.rows); mdl_->bind(); mdl_->issueBatch(m, batch_, bones_); glAssertError(); }
CameraInfo kinectPoseFromEigen(std::pair<Eigen::Matrix3d,Eigen::Vector3d> pos,float fx, float fy, float cx, float cy){ CameraInfo result; cv::Mat intrinsic = cv::Mat::eye(3,3,cv::DataType<double>::type); //Kinect Intrinsic Parameters intrinsic.at<double>(0,0) = fx; intrinsic.at<double>(1,1) = fy; intrinsic.at<double>(0,2) = cx; intrinsic.at<double>(1,2) = cy; result.setIntrinsic(intrinsic); Eigen::Matrix3d rotation = pos.first; cv::Mat rotation2 = cv::Mat::eye(3,3,cv::DataType<double>::type); for(int i=0;i<3;i++) for(int j=0;j<3;j++) rotation2.at<double>(i,j) = rotation(i,j); result.setRotation(rotation2); Eigen::Vector3d translation = pos.second; cv::Mat translation2 = cv::Mat::zeros(3,1,cv::DataType<double>::type); for(int i=0;i<3;i++) translation2.at<double>(i,0) = translation(i); result.setTranslation(translation2); return result; }
std::vector<v2_t> GetPointProjections(const CameraInfo &cam, const std::vector<PointData> &points, const std::vector<int> &indices, bool inside_only, int &num_inside) { int num_points = (int) indices.size(); std::vector<v2_t> projs; BoundingBox bbox = cam.GetBoundingBox(); num_inside = 0; for (int i = 0; i < num_points; i++) { int pidx = indices[i]; const PointData &p = points[pidx]; double proj[2]; #if 1 bool in_front = cam.Project(p.m_pos, proj); if (!in_front) continue; bool inside = bbox.Contains(proj[0], proj[1]); #else bool inside = cam.Project(p.m_pos, proj); #endif if (inside) num_inside++; if (inside_only && inside) projs.push_back(v2_new(proj[0], proj[1])); else if (!inside_only) projs.push_back(v2_new(proj[0], proj[1])); } return projs; }
virtual void render(CameraInfo const &cam, int pass) { glAssertError(); Matrix m; cam.getModelView(this, &m); m.setTranslation(Vec3(0, 0, 0)); mdl_->bind(); size_t cnt; TriangleBatch const *tb = mdl_->batches(&cnt); for (size_t i = 0; i != cnt; ++i) { mdl_->issueBatch(m, i, bones_); } }
CameraInfo OnlineFusionObject::fromSE3(pi::SE3d pose,float fx,float fy,float cx,float cy) { CameraInfo result; cv::Mat intrinsic = cv::Mat::eye(3,3,cv::DataType<double>::type); //Kinect Intrinsic Parameters intrinsic.at<double>(0,0) = fx; intrinsic.at<double>(1,1) = fy; intrinsic.at<double>(0,2) = cx; intrinsic.at<double>(1,2) = cy; result.setIntrinsic(intrinsic); cv::Mat rotation2 = cv::Mat::eye(3,3,cv::DataType<double>::type); // for(int i=0;i<3;i++) for(int j=0;j<3;j++) rotation2.at<double>(i,j) = rotation(i,j); pose.get_rotation().getMatrix((double*)rotation2.data); result.setRotation(rotation2); pi::Point3d trans=pose.get_translation(); cv::Mat translation2 = cv::Mat::zeros(3,1,cv::DataType<double>::type); translation2.at<double>(0,0) = trans.x; translation2.at<double>(1,0) = trans.y; translation2.at<double>(2,0) = trans.z; result.setTranslation(translation2); return result; }
QVector<CameraInfo> CameraUSB::avaibleCams(const CameraInfo &info) { QVector<CameraInfo> avaibleCams; libusb_context *ctx = NULL; libusb_device **devs; ssize_t cnt; int vendorId = info.getParam("vendorId").toString().toInt(0, 16); int productId = info.getParam("productId").toString().toInt(0, 16); // create context if(libusb_init(&ctx) != LIBUSB_SUCCESS) return avaibleCams; // get list usb device cnt = libusb_get_device_list(ctx, &devs); for(int i = 0; i < cnt; i++) { libusb_device_descriptor desc; if(libusb_get_device_descriptor(devs[i], &desc)==LIBUSB_SUCCESS) { if(desc.idVendor==vendorId && desc.idProduct==productId) { QString addr = QString("%1.%2").arg((int)libusb_get_bus_number(devs[i])) .arg((int)libusb_get_device_address(devs[i])); avaibleCams.append(CameraInfo("DreamCam USB", "USB", addr)); } } } libusb_free_device_list(devs, 1); // destroy usb context libusb_exit(ctx); return avaibleCams; }
std::vector<CameraInfo> getCamerasInfos() { std::vector<CameraInfo> camerasInfo; #ifdef AVG_ENABLE_1394_2 int amountFWCameras = FWCamera::countCameras(); for (int i = 0; i < amountFWCameras; i++) { CameraInfo* camInfo = FWCamera::getCameraInfos(i); if (camInfo != NULL) { camInfo->checkAddBayer8(); camerasInfo.push_back(*camInfo); } } #endif #ifdef AVG_ENABLE_CMU1394 int amountCMUCameras = CMUCamera::countCameras(); for (int i = 0; i < amountCMUCameras; i++) { CameraInfo* camInfo = CMUCamera::getCameraInfos(i); if (camInfo != NULL) { camInfo->checkAddBayer8(); camerasInfo.push_back(*camInfo); } } #endif #ifdef AVG_ENABLE_DSHOW int amountDSCameras = DSCamera::countCameras(); for (int i = 0; i < amountDSCameras; i++) { CameraInfo* camInfo = DSCamera::getCameraInfos(i); if (camInfo != NULL) { camInfo->checkAddBayer8(); camerasInfo.push_back(*camInfo); } } #endif #ifdef AVG_ENABLE_V4L2 int amountV4LCameras = V4LCamera::countCameras(); for (int i = 0; i < amountV4LCameras; i++) { CameraInfo* camInfo = V4LCamera::getCameraInfos(i); if (camInfo != NULL) { camInfo->checkAddBayer8(); camerasInfo.push_back(*camInfo); } } #endif return camerasInfo; }
bool CameraUSB::connect(const CameraInfo &info) { int ret; // com settings _vendorId = info.getParam("vendorId").toString().toInt(0, 16); _productId = info.getParam("productId").toString().toInt(0, 16); _epIn = info.getParam("EPIN").toString().toInt(0, 16); _epOut = info.getParam("EPOUT").toString().toInt(0, 16); _interfaceNumber = info.getParam("interface").toString().toInt(0, 16); if(_vendorId == 0 || _productId == 0) { qDebug()<<"USB bad vendorId/productId settings"; return false; } // connect if(info.addr().isEmpty()) { qDebug()<<"addr empty"; _devHandle = libusb_open_device_with_vid_pid(_ctx, _vendorId, _productId); } else { // get list usb device libusb_device **devs; ssize_t cnt = libusb_get_device_list(_ctx, &devs); for(int i = 0; i < cnt; i++) { libusb_device_descriptor desc; if(libusb_get_device_descriptor(devs[i], &desc)==LIBUSB_SUCCESS) { if(desc.idVendor==_vendorId && desc.idProduct==_productId) { QString addr = QString("%1.%2").arg((int)libusb_get_bus_number(devs[i])) .arg((int)libusb_get_device_address(devs[i])); if(addr==info.addr()) { _device = devs[i]; libusb_open(devs[i], &_devHandle); } } } } libusb_free_device_list(devs, 1); } if(_devHandle == NULL) { qDebug()<<"Cannot open device"<<endl; return false; } // reset device if((ret = libusb_reset_device(_devHandle)) != 0) qDebug()<<"Cannot reset device"<<libusb_strerror(libusb_error(ret)); if(libusb_kernel_driver_active(_devHandle, _interfaceNumber) == 1) //find out if kernel driver is attached { printf("Kernel Driver Active\n"); if(libusb_detach_kernel_driver(_devHandle, _interfaceNumber) == 0) //detach it printf("Kernel Driver Detached!\n"); } ret = libusb_claim_interface(_devHandle, _interfaceNumber); if(ret != 0) { qDebug()<<"Cannot claim device"<<libusb_strerror(libusb_error(ret)); return false; } if((ret = libusb_clear_halt(_devHandle, _epOut)) != 0) qDebug()<<"Cannot clear EPOUT"<<libusb_strerror(libusb_error(ret)); if((ret = libusb_clear_halt(_devHandle, _epIn)) != 0) qDebug()<<"Cannot clear EPIN"<<libusb_strerror(libusb_error(ret)); //libusb_set_debug(_ctx, LIBUSB_LOG_LEVEL_DEBUG); //flush(); return true; }
/* Read in information about the world */ void BaseApp::ReadBundleFile(const char *filename) { printf("[ReadBundleFile] Reading file...\n"); FILE *f = fopen(filename, "r"); if (f == NULL) { printf("Error opening file %s for reading\n", filename); return; } int num_images, num_points; char first_line[256]; fgets(first_line, 256, f); if (first_line[0] == '#') { double version; sscanf(first_line, "# Bundle file v%lf", &version); m_bundle_version = version; printf("[ReadBundleFile] Bundle version: %0.3f\n", version); fscanf(f, "%d %d\n", &num_images, &num_points); } else if (first_line[0] == 'v') { double version; sscanf(first_line, "v%lf", &version); m_bundle_version = version; printf("[ReadBundleFile] Bundle version: %0.3f\n", version); fscanf(f, "%d %d\n", &num_images, &num_points); } else { m_bundle_version = 0.1; sscanf(first_line, "%d %d\n", &num_images, &num_points); } printf("[ReadBundleFile] Reading %d images and %d points...\n", num_images, num_points); if (num_images != GetNumImages()) { printf("Error: number of images doesn't match file!\n"); return; } /* Read cameras */ for (int i = 0; i < num_images; i++) { double focal_length; double R[9]; double t[3]; double k[2] = { 0.0, 0.0 }; if (m_bundle_version >= 0.4) { char name[512]; int w, h; fscanf(f, "%s %d %d\n", name, &w, &h); } /* Focal length */ if (m_bundle_version > 0.1) { fscanf(f, "%lf %lf %lf\n", &focal_length, k+0, k+1); } else { fscanf(f, "%lf\n", &focal_length); } /* Rotation */ fscanf(f, "%lf %lf %lf\n%lf %lf %lf\n%lf %lf %lf\n", R+0, R+1, R+2, R+3, R+4, R+5, R+6, R+7, R+8); /* Translation */ fscanf(f, "%lf %lf %lf\n", t+0, t+1, t+2); #if 0 if (m_bundle_version < 0.3) { R[2] = -R[2]; R[5] = -R[5]; R[6] = -R[6]; R[7] = -R[7]; t[2] = -t[2]; } #endif if (focal_length <= 100.0 || m_image_data[i].m_ignore_in_bundle) { /* No (or bad) information about this camera */ m_image_data[i].m_camera.m_adjusted = false; } else { CameraInfo cd; cd.m_adjusted = true; cd.m_width = m_image_data[i].GetWidth(); cd.m_height = m_image_data[i].GetHeight(); cd.m_focal = focal_length; cd.m_k[0] = k[0]; cd.m_k[1] = k[1]; memcpy(cd.m_R, R, sizeof(double) * 9); memcpy(cd.m_t, t, sizeof(double) * 3); cd.Finalize(); m_image_data[i].m_camera = cd; } } /* Read points */ m_point_data.clear(); m_point_data.resize(num_points); int num_min_views_points = 0; for (int i = 0; i < num_points; i++) { PointData &pt = m_point_data[i]; /* Position */ fscanf(f, "%lf %lf %lf\n", pt.m_pos + 0, pt.m_pos + 1, pt.m_pos + 2); // if (m_bundle_version < 0.3) // pt.m_pos[2] = -pt.m_pos[2]; /* Color */ fscanf(f, "%f %f %f\n", pt.m_color + 0, pt.m_color + 1, pt.m_color + 2); int num_visible; fscanf(f, "%d", &num_visible); pt.m_num_vis=num_visible; if (num_visible >=3) num_min_views_points++; // pt.m_views.resize(num_visible); for (int j = 0; j < num_visible; j++) { int view, key; fscanf(f, "%d %d", &view, &key); if (!m_image_data[view].m_camera.m_adjusted) { // printf("[ReadBundleFile] " // "Removing view %d from point %d\n", view, i); } else { /* Check cheirality */ bool val = (m_bundle_version >= 0.3); double proj_test[2]; if (m_image_data[view].m_camera. Project(pt.m_pos, proj_test) == val) { pt.m_views.push_back(ImageKey(view, key)); } else { printf("[ReadBundleFile] " "Removing view %d from point %d [cheirality]\n", view, i); // pt.m_views.push_back(ImageKey(view, key)); } } // pt.m_views.push_back(ImageKey(view, key)); if (m_bundle_version >= 0.3) { double x, y; fscanf(f, "%lf %lf", &x, &y); } } // #define CROP_POINT_CLOUD #ifdef CROP_POINT_CLOUD const double x_min = 1.327; const double x_max = 3.556; const double y_min = -1.414; const double y_max = 1.074; const double z_min = -5.502; const double z_max = -3.288; if (pt.m_pos[0] < x_min || pt.m_pos[0] > x_max || pt.m_pos[1] < y_min || pt.m_pos[1] > y_max || pt.m_pos[2] < z_min || pt.m_pos[2] > z_max) { pt.m_views.clear(); } #endif /* CROP_POINT_CLOUD */ } #if 0 /* Read outliers */ int num_outliers; fscanf(f, "%d", &num_outliers); for (int i = 0; i < num_outliers; i++) { ImageKey ik; fscanf(f, "%d %d", &(ik.first), &(ik.second)); m_outliers.push_back(ik); } #endif fclose(f); printf("[ReadBundleFile] %d / %d points visible to more than 2 cameras!\n", num_min_views_points, num_points); }
/* Read in information about the "world" from the specified file. */ void BaseApp::ReadBundleFile(char *filename) { #ifdef _DEBUG_ printf("[BaseApp::ReadBundleFile] Reading file...\n"); #endif //--[1] See if file will open ... FILE *f = fopen(filename, "r"); if (f == NULL) { printf("Error opening file %s for reading\n", filename); return; } //--[2] If so, then process the header. Get #images and #points. int num_images, num_points; char first_line[256]; fgets(first_line, 256, f); if (first_line[0] == '#') { double version; sscanf(first_line, "# Bundle file v%lf", &version); m_bundle_version = version; fscanf(f, "%d %d\n", &num_images, &num_points); #ifdef _DEBUG_ printf("[ReadBundleFile] Bundle version: %0.3f\n", version); #endif } else if (first_line[0] == 'v') { double version; sscanf(first_line, "v%lf", &version); m_bundle_version = version; fscanf(f, "%d %d\n", &num_images, &num_points); #ifdef _DEBUG_ printf("[ReadBundleFile] Bundle version: %0.3f\n", version); #endif } else { m_bundle_version = 0.1; sscanf(first_line, "%d %d\n", &num_images, &num_points); } #ifdef _DEBUG_ printf("[BaseApp::ReadBundleFile] Reading %d images and %d points...\n", num_images, num_points); #endif if (num_images != GetNumImages()) // Compare to known info (sanity check). { printf("Error: number of images doesn't match file!\n"); return; } //--[3] Read in camera information. (One camera per image, yeah?) // Camera has focal length, two nonlinear parms, and extrinsic parms. for (int i = 0; i < num_images; i++) { double focal_length; double R[9]; double t[3]; double k[2] = { 0.0, 0.0 }; if (m_bundle_version >= 0.4) { char name[512]; int w, h; fscanf(f, "%s %d %d\n", name, &w, &h); } //--[3.1] Focal length if (m_bundle_version > 0.1) fscanf(f, "%lf %lf %lf\n", &focal_length, k+0, k+1); else fscanf(f, "%lf\n", &focal_length); //--[3.2] Rotation fscanf(f, "%lf %lf %lf\n%lf %lf %lf\n%lf %lf %lf\n", R+0, R+1, R+2, R+3, R+4, R+5, R+6, R+7, R+8); //--[3.3] Translation fscanf(f, "%lf %lf %lf\n", t+0, t+1, t+2); //--[3.4] If data is "sane" then aggregate data into camera info object. if (focal_length <= 100.0 || m_image_data[i].m_ignore_in_bundle) { /* No (or bad) information about this camera */ m_image_data[i].m_camera.m_adjusted = false; } else { CameraInfo cd; cd.m_adjusted = true; cd.m_width = m_image_data[i].GetWidth(); cd.m_height = m_image_data[i].GetHeight(); cd.m_focal = focal_length; cd.m_k[0] = k[0]; cd.m_k[1] = k[1]; memcpy(cd.m_R, R, sizeof(double) * 9); memcpy(cd.m_t, t, sizeof(double) * 3); cd.Finalize(); m_image_data[i].m_camera = cd; } } //--[4] Read in the points of the bundle/world point cloud. m_point_data.clear(); m_point_data.resize(num_points); int num_min_views_points = 0; for (int i = 0; i < num_points; i++) { PointData &pt = m_point_data[i]; //--[4.1] Position fscanf(f, "%lf %lf %lf\n", pt.m_pos + 0, pt.m_pos + 1, pt.m_pos + 2); //--[4.2] Color fscanf(f, "%f %f %f\n", pt.m_color + 0, pt.m_color + 1, pt.m_color + 2); //--[4.3] Visibility info (frame, keypoint index, coord location). int num_visible; fscanf(f, "%d", &num_visible); // How many images have this point? pt.m_num_vis=num_visible; if (num_visible >=3) // If more than three, add as good point. num_min_views_points++; for (int j = 0; j < num_visible; j++) { int view, key; fscanf(f, "%d %d", &view, &key); if (m_image_data[view].m_camera.m_adjusted) { /* Check chirality */ bool val = (m_bundle_version >= 0.3); double proj_test[2]; if (m_image_data[view].m_camera.Project(pt.m_pos, proj_test) == val) { pt.m_views.push_back(ImageKey(view, key)); } else { printf("[BaseApp::ReadBundleFile] " "Excluding view %d from point %d [chirality]\n", view, i); } } if (m_bundle_version >= 0.3) { double x, y; fscanf(f, "%lf %lf", &x, &y); } } // #define CROP_POINT_CLOUD #ifdef CROP_POINT_CLOUD const double x_min = 1.327; const double x_max = 3.556; const double y_min = -1.414; const double y_max = 1.074; const double z_min = -5.502; const double z_max = -3.288; if (pt.m_pos[0] < x_min || pt.m_pos[0] > x_max || pt.m_pos[1] < y_min || pt.m_pos[1] > y_max || pt.m_pos[2] < z_min || pt.m_pos[2] > z_max) pt.m_views.clear(); #endif /* CROP_POINT_CLOUD */ } fclose(f); #ifdef _DEBUG_ printf("[BaseApp::ReadBundleFile] %d / %d points visible to over 2 cameras!\n", num_min_views_points, num_points); #endif }
void LineSegment3D::Render(const CameraInfo &camera, double max_width, int stroke_texture, ParameterBound stroke_bounds) { #ifndef __BUNDLER__ #ifndef __DEMO__ #if 1 /* Project the line into the camera */ double p1[4] = { m_p1[0], m_p1[1], m_p1[2], 1.0 }; double p2[4] = { m_p2[0], m_p2[1], m_p2[2], 1.0 }; double proj1[3], proj2[3]; matrix_product(3, 4, 4, 1, (double *) camera.m_Pmatrix, p1, proj1); matrix_product(3, 4, 4, 1, (double *) camera.m_Pmatrix, p2, proj2); if (proj1[2] >= 0.0 || proj2[2] >= 0.0) return; double width = CLAMP(5.0 / (-proj1[2] - proj2[2]), 0.5, max_width); proj1[0] /= -proj1[2]; proj1[1] /= -proj1[2]; proj2[0] /= -proj2[2]; proj2[1] /= -proj2[2]; #endif #if 0 bool in_front1 = camera.Project(m_p1, proj1); bool in_front2 = camera.Project(m_p2, proj2); if (!in_front1 || !in_front2) return; #endif /* Draw a line segment whose width is proportional to the distance * from the camera */ #if 0 double pos[3]; camera.GetPosition(pos); double disp[3]; matrix_diff(3, 1, 3, 1, pos, m_p1, disp); double dist = matrix_norm(3, 1, disp); #endif if (stroke_texture != -1) { glEnable(GL_TEXTURE_2D); glBindTexture(GL_TEXTURE_2D, stroke_texture); } #if 0 #define LINE_WIDTH_SIGMA 1.0 double width = max_width * exp(-dist * dist / (LINE_WIDTH_SIGMA * LINE_WIDTH_SIGMA)); #endif /* Create a stroke */ Stroke s; s.radius() = 0.5 * width; s.cap() = false; s.depth() = 1.0; if (stroke_texture == -1) { s.useTexture() = false; s.color() = makeColor(0, 0, 0, 0.9f); } else { GLubyte b = 0x0; s.useTexture() = true; s.color() = makeColor(b, b, b, 0.9f); } s.addControlPoint(proj1[0], proj1[1]); s.addControlPoint(proj2[0], proj2[1]); s.render(); if (stroke_texture != -1) { glDisable(GL_TEXTURE_2D); } #endif /* __DEMO__ */ #endif /* __BUNDLER__ */ }