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__ */
}
