struct camera* db_select_cameras(int *ncams) {
int i;
int n_cameras;
struct camera* cameras;
PGresult *result = PQexec(conn, "SELECT id, url, code, analize_frames, threshold, motion_delay FROM cameras ORDER BY id;");
if(PQresultStatus(result) != PGRES_TUPLES_OK) {
fprintf(stderr, "No cameras found: %s\n", PQresultErrorMessage(result));
PQclear(result);
}
n_cameras = PQntuples(result);
cameras = (struct camera*)malloc(n_cameras * sizeof(struct camera));
memset(cameras, 0, n_cameras * sizeof(struct camera));
for(i=0; i < n_cameras; i++) {
char *id = PQgetvalue(result, i, 0);
char *url = PQgetvalue(result, i, 1);
char *name = PQgetvalue(result, i, 2);
char *analize_frames = PQgetvalue(result, i, 3);
char *threshold = PQgetvalue(result, i, 4);
char *motion_delay = PQgetvalue(result, i, 5);
cameras[i].url = (char*)malloc(strlen(url)+1);
cameras[i].name = (char*)malloc(strlen(name)+1);
strcpy(cameras[i].url, url);
strcpy(cameras[i].name, name);
sscanf(id, "%d", &cameras[i].id);
sscanf(analize_frames, "%d", &cameras[i].analize_frames);
sscanf(threshold, "%d", &cameras[i].threshold);
sscanf(motion_delay, "%d", &cameras[i].motion_delay);
cameras[i].last_screenshot = 0;
cameras[i].active = 1;
cameras[i].cam_consumers_list = NULL;
if(pthread_mutex_init(&cameras[i].consumers_lock, NULL) < 0) {
fprintf(stderr, "pthread_mutex_init failed\n");
exit(EXIT_FAILURE);
}
print_camera(&cameras[i]);
}
PQclear(result);
*ncams = n_cameras;
return cameras;
}
int main(int argc, char** argv)
{
google::InitGoogleLogging(argv[0]);
if (argc != 5)
{
std::cerr << "usage: monoExCal <3Dpoints_file> <observation_file> <intrinsic_file> <extrinsic_file>\n";
return 1;
}
int num_points;
int num_observations;
observation o;
// this code peforms extrinsic calibration on a monocular camera
// it assumes that 3D data from a positioning device is available
// this 3D data could come from an IGPS, a Fero arm, or any robot
// each 3D point should be observed by the camera, and the image(x,y) position
// of that observation must be known.
// It is assumed that the intrinsic calibration is already known
// The input is provided by 4 files
// 1. 3D points stored as ascii in the form:
// num_points # read as integer
// x[0] y[0] z[0] # read as double
// ...
// x[num_points-1] y[num_points-1] z[num_points-1]
// 2. Observations stored as ascii in the form
// num_observations # read as integer
// x[0] y[0] # read as double
// ...
// x[num_observations-1] y[num_observations-1]
// 3. Camera intrisic data stored as ascii in the ROS.ini format
// 4. Camera initial extrinsic file stored as ascii indicating the homogeneous transform
// nx ox ax tx #read all as double
// ny oy ay ty
// nz oz az tz
// 0 0 0 1.0
// read in the problem
FILE *points_fp = fopen(argv[1], "r");
FILE *observations_fp = fopen(argv[2], "r");
FILE *intrinsics_fp = fopen(argv[3], "r");
FILE *extrinsics_fp = fopen(argv[4], "r");
if (points_fp == NULL)
{
printf("Could not open file: %s", argv[1]);
exit(1);
}
if (observations_fp == NULL)
{
printf("Could not open file: %s", argv[2]);
exit(1);
}
if (intrinsics_fp == NULL)
{
printf("Could not open file: %s", argv[3]);
exit(1);
}
if (extrinsics_fp == NULL)
{
printf("Could not open file: %s", argv[4]);
exit(1);
}
// first read points file
if (fscanf(points_fp, "%d", &num_points) != 1)
{
printf("couldn't read num_points\n");
exit(1);
}
std::vector<point> Pts;
for (int i = 0; i < num_points; i++)
{
point p;
if (fscanf(points_fp, "%lf %lf %lf", &p.x, &p.y, &p.z) != 3)
{
printf("couldn't read point %d from %s\n", i, argv[1]);
exit(1);
}
Pts.push_back(p);
}
fclose(points_fp);
// Then read in the observations
if (fscanf(observations_fp, "%d", &num_observations) != 1)
{
printf("couldn't read num_observations\n");
exit(1);
}
if (num_observations != num_points)
{
printf("WARNING, num_points NOT EQUAL to num_observations\n");
}
std::vector<observation> Ob;
for (int i = 0; i < num_observations; i++)
{
if (fscanf(observations_fp, "%lf %lf", &o.x, &o.y) != 2)
{
printf("couldn't read observation %d from %s\n", i, argv[2]);
exit(1);
}
o.p_id = i;
Ob.push_back(o);
}
fclose(observations_fp);
// read camera intrinsics
Camera C;
char dum[255];
double Dum, Dum2, Dum3;
int image_width;
int image_height;
int rtn = 0;
rtn += fscanf(intrinsics_fp, "%s", dum); // should be "#"
rtn += fscanf(intrinsics_fp, "%s", dum); // should be "Camera"
rtn += fscanf(intrinsics_fp, "%s", dum); // should be "intrinsics"
rtn += fscanf(intrinsics_fp, "%s", dum); // should be "[image]"
// printf("should be [image]: %s\n",dum);
rtn += fscanf(intrinsics_fp, "%s", dum); // should be "width"
// printf("should be width: %s\n",dum);
rtn += fscanf(intrinsics_fp, "%d", &image_width); // should be the image width of provided by camera
printf("image_width: %d\n", image_width);
rtn += fscanf(intrinsics_fp, "%s", dum); // should be "height"
rtn += fscanf(intrinsics_fp, "%d", &image_height); // should be the image width of provided by camera
printf("height: %d\n", image_height);
rtn += fscanf(intrinsics_fp, "%s", dum); // should be "[some name]"
// printf("[some name]: %s\n",dum);
rtn += fscanf(intrinsics_fp, "%s", dum); // should be "camera"
// printf("should be camera: %s\n",dum);
rtn += fscanf(intrinsics_fp, "%s", dum); // should be "matrix"
// printf("should be matrix: %s\n",dum);
rtn += fscanf(intrinsics_fp, "%lf %lf %lf", &C.fx, &Dum, &C.cx);
rtn += fscanf(intrinsics_fp, "%lf %lf %lf", &Dum, &C.fy, &C.cy);
rtn += fscanf(intrinsics_fp, "%lf %lf %lf", &Dum, &Dum2, &Dum3);
rtn += fscanf(intrinsics_fp, "%s", dum); // should be "distortion"
printf("camera matrix:\n");
printf("%8.3lf %8.3lf %8.3lf\n", C.fx, 0.0, C.cx);
printf("%8.3lf %8.3lf %8.3lf\n", 0.0, C.fy, C.cy);
printf("%8.3lf %8.3lf %8.3lf\n", 0.0, 0.0, 0.0);
// printf("should be distortion: %s\n",dum);
rtn += fscanf(intrinsics_fp, "%lf %lf %lf %lf %lf", &C.k1, &C.k2, &C.k3, &C.p1, &C.p2);
printf("Distortion: [ %8.3lf %8.3lf %8.3lf %8.3lf %8.3lf ]\n", C.k1, C.k2, C.k3, C.p1, C.p2);
fclose(intrinsics_fp);
// read camera extrinsics
double H[4][4];
rtn = 0;
rtn += fscanf(extrinsics_fp, "%lf %lf %lf %lf", &H[0][0], &H[0][1], &H[0][2], &H[0][3]);
rtn += fscanf(extrinsics_fp, "%lf %lf %lf %lf", &H[1][0], &H[1][1], &H[1][2], &H[1][3]);
rtn += fscanf(extrinsics_fp, "%lf %lf %lf %lf", &H[2][0], &H[2][1], &H[2][2], &H[2][3]);
rtn += fscanf(extrinsics_fp, "%lf %lf %lf %lf", &H[3][0], &H[3][1], &H[3][2], &H[3][3]);
if (rtn != 16)
{
printf("could not read extrinsics rtn=%d from %s\n", rtn, argv[4]);
exit(1);
}
fclose(extrinsics_fp);
// use the inverse of transform from camera to world as camera transform
double HI[9]; // note ceres uses column major order
HI[0] = H[0][0]; // first column becomes first row
HI[1] = H[0][1];
HI[2] = H[0][2];
HI[3] = H[1][0]; // second column becomes second row
HI[4] = H[1][1];
HI[5] = H[1][2];
HI[6] = H[2][0]; // third column becomes third row
HI[7] = H[2][1];
HI[8] = H[2][2];
C.pos[0] = -(H[0][3] * H[0][0] + H[1][3] * H[1][0] + H[2][3] * H[2][0]);
C.pos[1] = -(H[0][3] * H[0][1] + H[1][3] * H[1][1] + H[2][3] * H[2][1]);
C.pos[2] = -(H[0][3] * H[0][2] + H[1][3] * H[1][2] + H[2][3] * H[2][2]);
printf("C.xyz = %lf %lf %lf\n", C.pos[0], C.pos[1], C.pos[2]);
ceres::RotationMatrixToAngleAxis(HI, C.aa);
/* used to create sample data with known solution
FILE *fp6 = fopen("new_observations.txt","w");
fprintf(fp6,"%d\n",num_points);
for(int i=0;i<num_points;i++){
o = project_point(C,Pts[i]);
fprintf(fp6,"%lf %lf\n",o.x,o.y);
}
fclose(fp6);
exit(1);
*/
#define MONO_EXCAL_DEBUG
#ifdef MONO_EXCAL_DEBUG
/* Print initial errors */
/* Save projections and observations to matlab compatible form */
FILE *fp_temp1 = fopen("Obs.m", "w");
FILE *fp_temp2 = fopen("Rep.m", "w");
FILE *fp_temp3 = fopen("FRep.m", "w");
fprintf(fp_temp1, "O = [ ");
fprintf(fp_temp2, "R = [ ");
fprintf(fp_temp3, "F = [ ");
for (int i = 0; i < num_points; i++)
{
o = project_point(C, Pts[i]);
printf("Errors %d = %lf %lf\n", i, Ob[i].x - o.x, Ob[i].y - o.y);
fprintf(fp_temp1, "%lf %lf;\n", Ob[i].x, Ob[i].y);
fprintf(fp_temp2, "%lf %lf;\n", o.x, o.y);
}
fprintf(fp_temp1, "];\n");
fprintf(fp_temp2, "];\n");
fclose(fp_temp1);
fclose(fp_temp2);
#endif // MONO_EXCAL_DEBUG
print_camera(C, "Original Parameters");
// Create residuals for each observation in the bundle adjustment problem. The
// parameters for cameras and points are added automatically.
ceres::Problem problem;
for (int i = 0; i < num_observations; ++i)
{
// Each Residual block takes a point and a camera as input and outputs a 2
// dimensional residual. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.
ceres::CostFunction* cost_function = Camera_reprj_error::Create(Ob[i].x, Ob[i].y);
problem.AddResidualBlock(cost_function, NULL, C.PB_extrinsics, C.PB_intrinsics, Pts[i].PB);
problem.SetParameterBlockConstant(C.PB_intrinsics);
problem.SetParameterBlockConstant(Pts[i].PB);
/* DEBUG the reprojection error, this shows how to call reprojection error directly
Camera_reprj_error CE(Ob[i].x,Ob[i].y);
double res[2];
CE(C.PB_extrinsics,C.PB_intrinsics,Pts[i].PB,res);
printf("residual %d = %9.3lf %9.3lf\n",i,res[0],res[1]);
*/
}
// Make Ceres automatically detect the bundle structure. Note that the
// standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
// for standard bundle adjustment problems.
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
options.max_num_iterations = 1000;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
#ifdef MONO_EXCAL_DEBUG
/* Print final errors */
for (int i = 0; i < num_points; i++)
{
o = project_point(C, Pts[i]);
printf("%d : Ob= %6.3lf %6.3lf ", i, Ob[i].x, Ob[i].y);
printf("%d : o= %6.3lf %6.3lf Errors = %10.3lf %10.3lf\n", i, o.x, o.y, Ob[i].x - o.x, Ob[i].y - o.y);
fprintf(fp_temp3, "%lf %lf;\n", o.x, o.y);
}
fprintf(fp_temp3, "];\n");
fclose(fp_temp3);
#endif // MONO_EXCAL_DEBUG
// Print final camera parameters
print_camera(C, "final parameters");
// write new extrinsics to a file
std::string temp(argv[4]);
std::string new_ex_file = "new_" + temp;
extrinsics_fp = fopen(new_ex_file.c_str(), "w");
ceres::AngleAxisToRotationMatrix(C.aa, HI); // Column Major
// invert HI to get H
H[0][0] = HI[0]; // first column of HI is set to first row of H
H[0][1] = HI[1];
H[0][2] = HI[2];
H[1][0] = HI[3]; // second column of HI is set to second row of H
H[1][1] = HI[4];
H[1][2] = HI[5];
H[2][0] = HI[6]; // third column of HI is set to third row of H
H[2][1] = HI[7];
H[2][2] = HI[8];
H[0][3] = -(C.pos[0] * HI[0] + C.pos[1] * HI[1] + C.pos[2] * HI[2]);
H[1][3] = -(C.pos[0] * HI[3] + C.pos[1] * HI[4] + C.pos[2] * HI[5]);
H[2][3] = -(C.pos[0] * HI[6] + C.pos[1] * HI[7] + C.pos[2] * HI[8]);
fprintf(extrinsics_fp, "%9.3lf %9.3lf %9.3lf %9.3lf\n", H[0][0], H[0][1], H[0][2], H[0][3]);
fprintf(extrinsics_fp, "%9.3lf %9.3lf %9.3lf %9.3lf\n", H[1][0], H[1][1], H[1][2], H[1][3]);
fprintf(extrinsics_fp, "%9.3lf %9.3lf %9.3lf %9.3lf\n", H[2][0], H[2][1], H[2][2], H[2][3]);
fprintf(extrinsics_fp, "%9.3lf %9.3lf %9.3lf %9.3lf\n", 0.0, 0.0, 0.0, 1.0);
fclose(extrinsics_fp);
return 0;
}
