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();
}
Esempio n. 3
0
 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]);
}
Esempio n. 10
0
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;
}