path: root/src/main.cpp

/*
 * =====================================================================================
 *
 *       Filename:  main.cpp
 *
 *    Description:  Main code for reflections SLAM. While the code is primarily
 *    designed to run in ROS, an attempt is being made to allow some
 *    functionality without it. However, no guarantees are made that the two
 *    will produce the same outputs for a given input.
 *
 *        Version:  1.0
 *        Created:  03/17/2017 04:12:33 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Martin Miller (MHM), miller7@illinois.edu
 *   Organization:  Aerospace Robotics and Controls Lab (ARC)
 *
 * =====================================================================================
 */

#include "main.h"
bool seenutm=false;
bool seenpva=false;
bool seencov=false;
using std::cout;
using std::endl;
using std::cerr;

double 
aboveWater(const Quaterniond &q)
{
    Vector3d tip;
    tip << 0.65*3.43, 0, -0.60;
    tip = q._transformVector(tip);
    return tip[2];
}

/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  covCallback
 *  Description:  Initializes position and velocity covariance in the proper
 *  coordinate frames. Rbw is the body to NED rotation. Renuned is the ENU->NED
 *  rotation. This is only set during the initialization phase and not during
 *  normal running.
 * =====================================================================================
 */
void
covCallback(const message &msg, State &mu, const Quaterniond &q)
{
    if (seencov && seenutm && seenpva) return;
    seencov=true;
    // Rotation from body to world
    Matrix3d Rbw(q.toRotationMatrix());
    // Rotation from ENU to NED
    Matrix3d Renuned;
    Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1;
    Matrix3d pcov = Renuned*msg.covariance.position*Renuned.transpose();
    pcov(2,2) *= 10;
    mu.position_covariance(pcov);
    mu.velocity_covariance(Rbw.transpose()*msg.covariance.velocity*Rbw);
}


/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  imgCallback
 *  Description:  Handles image measurements and update the filter. Only runs
 *  after the initialization phase.
 * =====================================================================================
 */
void
imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
{
    if (seenutm && seenpva && seencov) {
        std::vector<measurement_t> z;

        measurement_t height;
        height.z_type = HEIGHT;
        height.height = aboveWater(q);
        z.push_back(height);

        strcat(msg.image_names[0],".txt");
        FILE *fin;
        if ((fin=fopen(msg.image_names[0], "r"))==NULL) {
            err_sys("fopen: %s", msg.image_names[0]);
        }
        int id,sx,sy,rx,ry;
        while (fscanf(fin,"%d,%d,%d,%d,%d", &id, &sx, &sy, &rx, &ry)!=EOF) {
            measurement_t m;
            m.id = id;
            // Points in the image frame
            Vector3d xi, xir;
            xi << sx, sy, 1.;
            xir << rx, ry, 1.;
            // Points in the camera frame
            m.source = cam.img2body(xi);
            m.reflection = cam.img2body(xir);
            m.z_type = REFLECTION;
            z.push_back(m);
        }
        if (fclose(fin)==EOF) {
            err_sys("fclose");
        }
        mu.handle_measurements(z,q);
    }

    return;
}

/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  imuCallback
 *  Description:  Only runs after the initialization is complete. Updates the
 *  motion model, P_k|k-1, and removes features that leave the FOV.
 * =====================================================================================
 */
void 
imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt)
{
    if (!seenutm || !seenpva || !seencov || (dt.secs==0 && dt.nsecs==0)) return;
    Vector3d acc(msg.linear_acceleration.y,msg.linear_acceleration.x,-msg.linear_acceleration.z);
    Vector3d ang(msg.angular_velocity.y,msg.angular_velocity.x,-msg.angular_velocity.z);
    // WARNING: This is the bias for 1112-1
    Vector3d ang_bias(-2.795871394666666222e-03, 6.984255690000021506e-03, -1.418145565750002614e-03);
    ang -= ang_bias;
    double dtf = dt.secs+dt.nsecs*1e-9;

    mu.motionModel(acc,ang,q,dtf);
}

/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  pvaCallback
 *  Description:  During the initialization phase, updates the velocity and
 *  quaternion. Afterwards, updates only the quaternion.
 * =====================================================================================
 */
void
pvaCallback(const message &msg, State &mu, Quaterniond &q)
{
    // Update quaternion
    using Eigen::AngleAxisd;
    double roll, pitch, yaw;
    attitude_t A=msg.attitude;
    roll = M_PI*msg.attitude.roll/180.;
    pitch = M_PI*msg.attitude.pitch/180.;
    yaw = M_PI*(msg.attitude.yaw)/180.;
    Matrix3d R;
    R = AngleAxisd(yaw, Vector3d::UnitZ())
      * AngleAxisd(roll, Vector3d::UnitX())
      *  AngleAxisd(pitch, Vector3d::UnitY());
    q = Quaterniond(R);

    // Set body velocity
    if (!seenpva || !seencov || !seenutm) {
        Vector3d vw(msg.velocity.north,msg.velocity.east,-msg.velocity.up);
        mu.vel(R.transpose()*vw);
        seenpva=true;
    } 
    return;
}

/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  utmCallback
 *  Description:  During the initialization, updates the position. Does nothing
 *  during normal running.
 * =====================================================================================
 */
void
utmCallback(const message &msg, State &mu, const Quaterniond &q)
{
    static int i=0;
    if ((!seenutm || !seencov) && seenpva) {
        seenutm=true;
        UTM utm_water;
        utm_water.northing = msg.utm.northing;
        utm_water.easting = msg.utm.easting;
        utm_water.up = -aboveWater(q);
        utm_water.zone_i = msg.utm.zone_i;
        utm_water.zone_c = msg.utm.zone_c;
        mu.pos(utm_water);
    } else {
        /*
        i+=1;
        if (i%18==0) {
            UTM utm_water;
            utm_water.northing = msg.utm.northing;
            utm_water.easting = msg.utm.easting;
            utm_water.up = -aboveWater(q);
            utm_water.zone_i = msg.utm.zone_i;
            utm_water.zone_c = msg.utm.zone_c;
            mu.pos(utm_water);
        }
        */
    }
    return;
}

/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  parseLine
 *  Description:  Parses a line from regular file input. Gets the time,
 *  determines the message type and fills in the corresponding data fields.
 * =====================================================================================
 */
int 
parseLine(char *line, message *data)
{
    char *time_string;
    char *msg_type;
    double t;
    int s,ns;
    time_string = strsep(&line, ",");
    msg_type = strsep(&line, ",");
    sscanf(time_string,"%lf", &t);
    s = (int) t;
    ns = (int) ((t-s)*1e9);
    data->stamp.secs=s;
    data->stamp.nsecs=ns;

    if (!strcmp(msg_type,"bestutm")) {
        data->msg_type = BESTUTM;
        sscanf(line,"%d,%c,%lf,%lf,%lf", 
                &data->utm.zone_i, 
                &data->utm.zone_c, 
                &data->utm.northing, 
                &data->utm.easting, 
                &data->utm.up);
    } else if (!strcmp(msg_type,"IMG")) {
        data->msg_type = IMG;
        char *lfn;
        lfn = strsep(&line, ",");
        sscanf(lfn, "%s", &data->image_names);
        sscanf(line, "%s", &data->image_names[1]);
    } else if (!strcmp(msg_type,"inscovs")) {
        double pos[9];
        double att[9];
        double vel[9];
        data->msg_type = INSCOVS;
        sscanf(line,"%lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf,\
                     %lf,%lf,%lf",
                     pos, pos+1, pos+2,
                     pos+3, pos+4, pos+5,
                     pos+6, pos+7, pos+8,
                     att, att+1, att+2,
                     att+3, att+4, att+5,
                     att+6, att+7, att+8,
                     vel, vel+1, vel+2,
                     vel+3, vel+4, vel+5,
                     vel+6, vel+7, vel+8);
        data->covariance.position << pos[0], pos[1], pos[2],
                                     pos[3], pos[4], pos[5],
                                     pos[6], pos[7], pos[8];
        data->covariance.attitude << att[0], att[1], att[2],
                                     att[3], att[4], att[5],
                                     att[6], att[7], att[8];
        data->covariance.velocity << vel[0], vel[1], vel[2],
                                     vel[3], vel[4], vel[5],
                                     vel[6], vel[7], vel[8];
    } else if (!strcmp(msg_type,"inspvas")) {
        data->msg_type = INSPVAS;
        sscanf(line,"%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
                &data->position.latitude,
                &data->position.longitude,
                &data->position.altitude,
                &data->velocity.north,
                &data->velocity.east,
                &data->velocity.up,
                &data->attitude.roll,
                &data->attitude.pitch,
                &data->attitude.yaw);
    } else if (!strcmp(msg_type,"rawimus")) {
        data->msg_type = RAWIMUS;
        double acc_y, ang_y;
        sscanf(line,"%lf,%lf,%lf,%lf,%lf,%lf",
                &data->linear_acceleration.z,
                &acc_y,
                &data->linear_acceleration.x,
                &data->angular_velocity.z,
                &ang_y,
                &data->angular_velocity.x);
        data->linear_acceleration.y = -acc_y;
        data->angular_velocity.y = -ang_y;
    } else {
        fprintf(stderr, "Message type %s is unknown, quitting.\n", msg_type);
        exit(1);
    }
}

/* 
 * ===  FUNCTION  ======================================================================
 *         Name:  update_dt
 *  Description:  Determines dt based on the current and previous timestamp.
 * =====================================================================================
 */
timestamp 
update_dt(const timestamp t, timestamp *t_old)
{
    double dtf=0;
    if (t_old->secs!=0 || t_old->nsecs!=0) {
        double tf = t.secs+1e-9*t.nsecs;
        double t_oldf = t_old->secs+1e-9*t_old->nsecs;
        dtf = tf-t_oldf;
    }
    t_old->secs=t.secs;
    t_old->nsecs=t.nsecs;

    timestamp dt;
    dt.secs = (int) dtf;
    dt.nsecs = (int)((dtf-(double)dt.secs)*1e9);
    
    return dt;
}

int 
main(int argc, char **argv)
{
    if (argc!=2) {
        fprintf(stderr, "Usage: %s camera\n", argv[0]);
        exit(1);
    }
    State mu;
    Quaterniond qbw;
    // bias in FRD coordinates
    Vector3d acc_bias;
    //acc_bias << -0.03713532, 1.125*0.01465135, -1*-0.00709229;
    acc_bias << 0.95*-0.03713532, 0.65*0.01465135, -1*-0.00709229;
    mu.accelerometer_bias(acc_bias);
    timestamp dt{0,0};
    timestamp t_old{0,0};

    // Open camera YAML
    Camera cam(argv[1]);

#ifdef  ROS_PUBLISH
    // Create ROS publisher
    ros::init(argc, argv, "refslam");
    ros::NodeHandle n;
    ros::Publisher ekfPose = n.advertise<geometry_msgs::PoseStamped>("ekfPose", 100);
#else      /* -----  not ROS_PUBLISH  ----- */
    // Print unicsv header
    printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z\n");
#endif     /* -----  not ROS_PUBLISH  ----- */
    // Read sensors from file
    char line[MAXLINE];
    while (scanf("%s", line)!=EOF) {
        message msg;
        parseLine(line, &msg);
        
        switch ( msg.msg_type ) {
            case BESTUTM:	
                utmCallback(msg, mu,qbw);
                break;

            case IMG:	
                imgCallback(msg, mu, cam, qbw);
                break;

            case INSPVAS:	
                pvaCallback(msg, mu, qbw);
                break;

            case RAWIMUS:	
                dt = update_dt(msg.stamp, &t_old);
                imuCallback(msg, mu, qbw, dt);
                if (seenutm && seencov && seenpva) {
#ifdef  ROS_PUBLISH
                    geometry_msgs::PoseStamped msg;
                    ros::spinOnce();
#else
                    mu.unicsv();
#endif
                }
                break;

            case INSCOVS:
                covCallback(msg, mu, qbw);
                break;

            default:	
                break;
        }				/* -----  end switch  ----- */
    }
	return 0;
}