double OmniCamera::
computeErrorMultiplier()
{
Vector3d vector1 = cam2world( .5*width_, .5*height_ );
Vector3d vector2 = cam2world( .5*width_ + .5, .5*height_ );
vector1 = vector1/vector1.norm();
vector2 = vector2/vector2.norm();
double factor1 = .5/( 1 - vector1.dot(vector2) );
vector1 = cam2world( width_, .5*height_ );
vector2 = cam2world( -.5 + (double) width_ , .5*height_ );
vector1 = vector1/vector1.norm();
vector2 = vector2/vector2.norm();
double factor2 = .5/( 1 - vector1.dot(vector2) );
return ( factor2 + factor1 ) * .5;
}
bool EdgeSE3Expmap::write(std::ostream& os) const {
SE3Quat cam2world(measurement().inverse());
for (int i=0; i<7; i++)
os << cam2world[i] << " ";
for (int i=0; i<6; i++)
for (int j=i; j<6; j++) {
os << " " << information()(i,j);
}
return os.good();
}
bool VertexSE3Expmap::write(std::ostream& os) const
{
SE3Quat cam2world(estimate().inverse());
for (int i=0; i<7; i++)
os << cam2world[i] << " ";
for (int i=0; i<2; i++)
os << _focal_length[i] << " ";
for (int i=0; i<2; i++)
os << _principle_point[i] << " ";
os << _baseline << " ";
return os.good();
}
bool EdgeSim3::write(std::ostream& os) const
{
Sim3 cam2world(measurement().inverse());
Vector7d v7 = cam2world.log();
for (int i=0; i<7; i++)
{
os << v7[i] << " ";
}
for (int i=0; i<7; i++)
for (int j=i; j<7; j++){
os << " " << information()(i,j);
}
return os.good();
}
bool VertexSim3Expmap::write(std::ostream &os) const {
Sim3 cam2world(estimate().inverse());
Vector7d lv = cam2world.log();
for (int i = 0; i < 7; i++) {
os << lv[i] << " ";
}
for (int i = 0; i < 2; i++) {
os << _focal_length1[i] << " ";
}
for (int i = 0; i < 2; i++) {
os << _principle_point1[i] << " ";
}
return os.good();
}
bool EdgeSim3::read(std::istream& is)
{
Vector7d v7;
for (int i=0; i<7; i++){
is >> v7[i];
}
Sim3 cam2world(v7);
setMeasurement(cam2world.inverse());
for (int i=0; i<7; i++)
for (int j=i; j<7; j++)
{
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
bool VertexSE3Expmap::write(std::ostream& os) const {
SE3Quat cam2world(estimate().inverse());
for (int i=0; i<7; i++)
os << cam2world[i] << " ";
return os.good();
}
//---------------------------------------------------------------------------
int main(int argc, char *argv[])
{
struct ocam_model o_cata;
get_ocam_model(&o1, "calib_results_fisheye1.txt");
get_ocam_model(&o2, "calib_results_fisheye2.txt");
int i;
printf("pol =\n"); for (i=0; i<o1.length_pol; i++){ printf("\t%e\n",o1.pol[i]); }; printf("\n");
printf("invpol =\n"); for (i=0; i<o1.length_invpol; i++){ printf("\t%e\n",o1.invpol[i]); }; printf("\n");
printf("\nxc = %f\nyc = %f\n\nwidth = %d\nheight = %d\n",o1.xc,o1.yc,o1.width,o1.height);
/* --------------------------------------------------------------------*/
/* WORLD2CAM projects 3D point into the image */
/* --------------------------------------------------------------------*/
double point3D[3] = { 100 , 200 , -300 }; // a sample 3D point
double point2D[2]; // the image point in pixel coordinates
world2cam(point2D, point3D, &o1); // The behaviour of this function is the same as in MATLAB
printf("\nworld2cam: pixel coordinates reprojected onto the image1\n");
printf("m_row= %2.4f, m_col=%2.4f\n", point2D[0], point2D[1]);
cam2world(point3D, point2D, &o1);
printf("\ncam2world: coordinates back-projected onto the unit sphere1 (x^2+y^2+z^2=1)\n");
printf("x= %2.4f, y=%2.4f, z=%2.4f\n", point3D[0], point3D[1], point3D[2]);
/*cam2world(point3D, point2D, &o2);
printf("\ncam2world: coordinates back-projected onto the unit sphere2 (x^2+y^2+z^2=1)\n");
printf("x= %2.4f, y=%2.4f, z=%2.4f\n", point3D[0], point3D[1], point3D[2]);*/
/* --------------------------------------------------------------------*/
src1 = imread("./img_l.jpg" );
src2 = imread("./img_r.jpg" );
dst_persp1 = cvCreateImage( src1.size(), 8, 3 );
dst_persp2 = cvCreateImage( src2.size(), 8, 3 );
mapx_persp1 = cvCreateMat(src1.rows, src1.cols, CV_32FC1);
mapy_persp1 = cvCreateMat(src1.rows, src1.cols, CV_32FC1);
mapx_persp2 = cvCreateMat(src2.rows, src2.cols, CV_32FC1);
mapy_persp2 = cvCreateMat(src2.rows, src2.cols, CV_32FC1);
//namedWindow("rectified image1", 1);
//namedWindow("rectified image2", 1);
//createTrackbar("scaling factor", "rectified image1", &sf, 25, onTrackbar1);
//createTrackbar("scaling factor", "rectified image2", &sf, 25, onTrackbar2);
//onTrackbar1(0, 0);
//onTrackbar2(0, 0);
create_perspecive_undistortion_LUT( mapx_persp1, mapy_persp1, &o1, sf );
mapx_persp_left = Mat(mapx_persp1); // to copy the data
mapy_persp_left = Mat(mapy_persp1); // to copy the data
//for( int j = 0; j < mapx_persp_left.rows; j++ ){
// for( int i = 924; i < mapx_persp_left.cols; i++ ){
// mapx_persp_left.at<float>(j,i) = 0 ;
// mapy_persp_left.at<float>(j,i) = 0 ;
//
// }
// }
remap(src1, dst_persp1, mapx_persp_left, mapy_persp_left, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0,0,0) );
cout<<"image1 process"<<endl;
//create_perspecive_undistortion_LUT( mapx_persp2, mapy_persp2, &o2, sf );
//mapx_persp_right = Mat(mapx_persp2); // to copy the data
//mapy_persp_right = Mat(mapy_persp2); // to copy the data
//remap(src2, dst_persp2, mapx_persp_right, mapy_persp_right, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0,0,0) );
imshow( "rectified image1", dst_persp1 );
//imshow( "rectified image2", dst_persp2 );
cvWaitKey();
return 0;
}
Vector3d OmniCamera::
cam2world (const Vector2d& px) const
{
return cam2world(px[0], px[1]);
}
float PerspectiveCamera::generate_ray(const Point &p, Ray *ray) {
*ray = Ray(Point(0,0,0), Vector(p), 0.f, INFINITY);
*ray = cam2world(*ray);
return 1.f;
}