/*
 * =====================================================================================
 *
 *       Filename:  test_camera.cpp
 *
 *    Description:  Tests the methods of the Camera class
 *
 *        Version:  1.0
 *        Created:  03/27/2017 12:19:51 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Martin Miller (MHM), miller7@illinois.edu
 *   Organization:  Aerospace Robotics and Controls Lab (ARC)
 *
 * =====================================================================================
 */


#include <iostream>
#include "camera.h"
#define TOL 1e-12

using std::cout;
using std::cerr;
using std::endl;
using namespace Eigen;

int main(int argc, char **argv)
{
    Camera cam("left.yml");
    MatrixXd k;
    k = cam.K();
    assert(k.cols()==3);
    assert(k.rows()==3);

    k = cam.K4();
    assert(k.cols()==4);
    assert(k.rows()==4);

    Vector3d xi,xb;
    // Feature 114 0000009362.bmp.txt
    // from pyslam: [[ 0.9237706 ] [-0.36529118] [-0.11493579]]
    Vector3d xb0;
    xb0 <<  0.9237706, -0.36529118, -0.11493579;
    xi << 236,216,1;
    xb = cam.img2body(xi);
    xb -= xb0;
    assert(abs(xb[0])<TOL);
    assert(abs(xb[1])<TOL);
    assert(abs(xb[2])<TOL);
	return 0;
}