bool G2oSlamInterface::printVertex(OptimizableGraph::Vertex* v)
{
static char buffer[10000]; // that should be more than enough
int vdim = v->dimension();
if (vdim == 3) {
char* s = buffer;
OnlineVertexSE2* v2 = static_cast<OnlineVertexSE2*>(v);
memcpy(s, "VERTEX_XYT ", 11);
s += 11;
s += modp_itoa10(v->id(), s);
*s++ = ' ';
s += modp_dtoa(v2->updatedEstimate.translation().x(), s, 6);
*s++ = ' ';
s += modp_dtoa(v2->updatedEstimate.translation().y(), s, 6);
*s++ = ' ';
s += modp_dtoa(v2->updatedEstimate.rotation().angle(), s, 6);
*s++ = '\n';
cout.write(buffer, s - buffer);
return true;
}
else if (vdim == 6) {
char* s = buffer;
OnlineVertexSE3* v3 = static_cast<OnlineVertexSE3*>(v);
Vector3d eulerAngles = internal::toEuler(v3->updatedEstimate.matrix().topLeftCorner<3,3>());
const double& roll = eulerAngles(0);
const double& pitch = eulerAngles(1);
const double& yaw = eulerAngles(2);
memcpy(s, "VERTEX_XYZRPY ", 14);
s += 14;
s += modp_itoa10(v->id(), s);
*s++ = ' ';
s += modp_dtoa(v3->updatedEstimate.translation().x(), s, 6);
*s++ = ' ';
s += modp_dtoa(v3->updatedEstimate.translation().y(), s, 6);
*s++ = ' ';
s += modp_dtoa(v3->updatedEstimate.translation().z(), s, 6);
*s++ = ' ';
s += modp_dtoa(roll, s, 6);
*s++ = ' ';
s += modp_dtoa(pitch, s, 6);
*s++ = ' ';
s += modp_dtoa(yaw, s, 6);
*s++ = '\n';
cout.write(buffer, s - buffer);
return true;
}
return false;
}
static enum writing_status open_new_log_file(struct logging_status *ls)
{
pr_debug("Logging: Opening new log file\r\n");
int i;
for (i = 0; i < MAX_LOG_FILE_INDEX; i++) {
char buf[12];
modp_itoa10(i, buf);
strcpy(ls->name, "rc_");
strcat(ls->name, buf);
strcat(ls->name, ".log");
const FRESULT res = f_open(g_logfile, ls->name,
FA_WRITE | FA_CREATE_NEW);
if ( FR_OK == res )
return WRITING_ACTIVE;
f_close(g_logfile);
}
/* We fail if here. Be sure to clean up name buffer.*/
ls->name[0] = '\0';
return WRITING_INACTIVE;
}
size_t ev3_write_int( const char *fn, int value )
{
char s[ 12 ];
modp_itoa10( value, s );
return ev3_write( fn, s );
}
bool HogmanSlamInterface::printVertex(PoseGraph2D::Vertex* v)
{
char* s = buffer;
memcpy(s, "VERTEX_XYT ", 11);
s += 11;
s += modp_itoa10(v->id(), s);
*s++ = ' ';
s += modp_dtoa(v->transformation.translation().x(), s, 6);
*s++ = ' ';
s += modp_dtoa(v->transformation.translation().y(), s, 6);
*s++ = ' ';
s += modp_dtoa(v->transformation.rotation().angle(), s, 6);
*s++ = '\n';
cout.write(buffer, s - buffer);
return true;
}
bool HogmanSlamInterface::printVertex(PoseGraph3D::Vertex* v)
{
char* s = buffer;
memcpy(s, "VERTEX_XYZRPY ", 14);
s += 14;
s += modp_itoa10(v->id(), s);
*s++ = ' ';
// TODO verify that our conversion from Euler angles is the same as given in the example code on the webpage
Vector6 p = v->transformation.toVector();
for (int i = 0; i < p.size(); ++i) {
s += modp_dtoa(p[i], s, 6);
*s++ = ' ';
}
*s++ = '\n';
cout.write(buffer, s - buffer);
return true;
}
static void appendInt(int num)
{
char buf[12];
modp_itoa10(num,buf);
appendFileBuffer(buf);
}
SEXP R_num_to_char(SEXP x, SEXP digits, SEXP na_as_string, SEXP use_signif) {
int len = length(x);
int na_string = asLogical(na_as_string);
int signif = asLogical(use_signif);
char buf[32];
SEXP out = PROTECT(allocVector(STRSXP, len));
if(isInteger(x)){
for (int i=0; i<len; i++) {
if(INTEGER(x)[i] == NA_INTEGER){
if(na_string == NA_LOGICAL){
SET_STRING_ELT(out, i, NA_STRING);
} else if(na_string){
SET_STRING_ELT(out, i, mkChar("\"NA\""));
} else {
SET_STRING_ELT(out, i, mkChar("null"));
}
} else {
modp_itoa10(INTEGER(x)[i], buf);
SET_STRING_ELT(out, i, mkChar(buf));
}
}
} else if(isReal(x)) {
int precision = asInteger(digits);
double * xreal = REAL(x);
for (int i=0; i<len; i++) {
double val = xreal[i];
if(!R_FINITE(val)){
if(na_string == NA_LOGICAL){
SET_STRING_ELT(out, i, NA_STRING);
} else if(na_string){
if(ISNA(val)){
SET_STRING_ELT(out, i, mkChar("\"NA\""));
} else if(ISNAN(val)){
SET_STRING_ELT(out, i, mkChar("\"NaN\""));
} else if(val == R_PosInf){
SET_STRING_ELT(out, i, mkChar("\"Inf\""));
} else if(val == R_NegInf){
SET_STRING_ELT(out, i, mkChar("\"-Inf\""));
} else {
error("Unrecognized non finite value.");
}
} else {
SET_STRING_ELT(out, i, mkChar("null"));
}
} else if(precision == NA_INTEGER){
snprintf(buf, 32, "%.15g", val);
SET_STRING_ELT(out, i, mkChar(buf));
} else if(signif){
//use signifant digits rather than decimal digits
snprintf(buf, 32, "%.*g", (int) ceil(fmin(15, precision)), val);
SET_STRING_ELT(out, i, mkChar(buf));
} else if(precision > -1 && precision < 10 && fabs(val) < 2147483647 && fabs(val) > 1e-5) {
//preferred method: fast with fixed decimal digits
//does not support large numbers or scientific notation
modp_dtoa2(val, buf, precision);
SET_STRING_ELT(out, i, mkChar(buf));
//Rprintf("Using modp_dtoa2\n");
} else {
//fall back on sprintf (includes scientific notation)
//limit total precision to 15 significant digits to avoid noise
//funky formula is mostly to convert decimal digits into significant digits
snprintf(buf, 32, "%.*g", (int) ceil(fmin(15, fmax(1, log10(val)) + precision)), val);
SET_STRING_ELT(out, i, mkChar(buf));
//Rprintf("Using sprintf with precision %d digits\n",(int) ceil(fmin(15, fmax(1, log10(val)) + precision)));
}
}
} else {
error("num_to_char called with invalid object type.");
}
UNPROTECT(1);
return out;
}
void putIntCell(Serial *serial, int num)
{
char buf[10];
modp_itoa10(num,buf);
putsCell(serial, buf);
}
