/* * ===================================================================================== * * 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]; } #ifdef USE_ROS #else /* ----- not USE_ROS ----- */ /* * === 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 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); } } #endif /* ----- not USE_ROS ----- */ /* * === 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 USE_ROS #else /* ----- not USE_ROS ----- */ printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z\n"); // 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) mu.unicsv(); break; case INSCOVS: covCallback(msg, mu, qbw); break; default: break; } /* ----- end switch ----- */ } #endif /* ----- not USE_ROS ----- */ return 0; }