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 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;
}
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 appendDouble(double num, int precision)
{
char buf[30];
modp_dtoa(num, buf, precision);
appendFileBuffer(buf);
}
int _tmain(int argc, _TCHAR* argv[])
{
const size_t max_num = 1000000000;
const int buffer_element_count = 100000;
const int max_float_digits = 8;
// Init critical section;
InitializeCriticalSection(&g_write_queue_cs);
g_write_queue_has_more_data_event = CreateEvent(NULL, FALSE, FALSE, NULL);
g_write_queue_accepts_more_data_event = CreateEvent(NULL, FALSE, TRUE, NULL);
HANDLE hFile = ::CreateFile(L"output.txt", GENERIC_WRITE, 0, 0, CREATE_ALWAYS, 0, NULL);
if (hFile == INVALID_HANDLE_VALUE)
{
printf("Oppps");
exit(-1);
}
// Launch a writer thread.
HANDLE hThread = CreateThread(NULL, 0, &WriteThreadProc, hFile,0, 0);
// std::ofstream output();
for (size_t i=0; i < max_num / buffer_element_count; ++i)
{
// Prepare a block of numbers for writing.
WriteBuffer* write_buffer = new WriteBuffer(buffer_element_count, max_float_digits);
char* write_ptr = write_buffer->ptr_;
for (int k = 0; k < buffer_element_count; ++k)
{
// Format each float to string and append to buffer.
float random = float(rand()) / RAND_MAX;
write_ptr += modp_dtoa(random, write_ptr, max_float_digits);
*(write_ptr++) = '\r';
*(write_ptr++) = '\n';
}
// Compute how many bytes to write.
write_buffer->useful_data_size_ = write_ptr - write_buffer->ptr_;
// Enqueue for writing.
while (write_buffer) {
EnterCriticalSection(&g_write_queue_cs);
if (g_write_queue.size() < kMaxQueue)
{
// ops.
g_write_queue.push(write_buffer);
SetEvent(g_write_queue_has_more_data_event);
write_buffer = NULL;
}
LeaveCriticalSection(&g_write_queue_cs);
if (write_buffer) {
// slow down writing, queue is full
printf("S");
WaitForSingleObject(g_write_queue_accepts_more_data_event, 200);
}
}
}
// Let the writing thread know we are done.
g_done = true;
SetEvent(g_write_queue_has_more_data_event);
// Wait for writing thread to finish.
WaitForSingleObject(hThread, INFINITE);
::CloseHandle(hFile);
return 0;
}
int _tmain(int argc, _TCHAR* argv[])
{
const size_t max_num = 100000000;
const int buffer_element_count = 10000;
const int max_float_digits = 8;
// Init critical section;
InitializeCriticalSection(&g_write_queue_cs);
g_write_queue_has_more_data_event = CreateEvent(NULL, FALSE, FALSE, NULL);
g_write_queue_accepts_more_data_event = CreateEvent(NULL, FALSE, TRUE, NULL);
HANDLE hFile = ::CreateFile(L"output.txt", GENERIC_WRITE, 0, 0, CREATE_ALWAYS, 0, NULL);
if (hFile == INVALID_HANDLE_VALUE)
{
printf("Oppps");
exit(-1);
}
// Launch a writer thread.
HANDLE hThread = CreateThread(NULL, 0, &WriteThreadProc, hFile,0, 0);
sfmt_t sfmt;
sfmt_init_gen_rand(&sfmt, 1234);
uint32_t* randoms = (uint32_t*) _aligned_malloc(sizeof(uint32_t)*buffer_element_count, 32);
g_begin_ticks = GetTickCount64();
// std::ofstream output();
int finish = max_num / buffer_element_count;
for (size_t i=0; i < finish; ++i)
{
// Prepare a block of numbers for writing.
WriteBuffer* write_buffer = new WriteBuffer(buffer_element_count, max_float_digits);
char* write_ptr = write_buffer->ptr_;
sfmt_fill_array32(&sfmt, randoms, buffer_element_count);
for (int k = 0; k < buffer_element_count; ++k)
{
// Format each float to string and append to buffer.
// float random = float(rand()) / RAND_MAX;
float random = float(randoms[k]) / 4294967296.0f;
write_ptr += modp_dtoa(random, write_ptr, max_float_digits);
*(write_ptr++) = '\r';
*(write_ptr++) = '\n';
}
// Compute how many bytes to write.
write_buffer->useful_data_size_ = write_ptr - write_buffer->ptr_;
// Enqueue for writing.
while (write_buffer) {
EnterCriticalSection(&g_write_queue_cs);
if (g_write_queue.size() < kMaxQueue)
{
// ops.
g_write_queue.push(write_buffer);
SetEvent(g_write_queue_has_more_data_event);
write_buffer = NULL;
}
LeaveCriticalSection(&g_write_queue_cs);
if (write_buffer) {
// slow down writing, queue is full
printf("S");
WaitForSingleObject(g_write_queue_accepts_more_data_event, 200);
}
}
}
g_end_ticks = GetTickCount64();
// Let the writing thread know we are done.
EnterCriticalSection(&g_write_queue_cs);
g_done = true;
LeaveCriticalSection(&g_write_queue_cs);
SetEvent(g_write_queue_has_more_data_event);
// Wait for writing thread to finish.
WaitForSingleObject(hThread, INFINITE);
_aligned_free(randoms);
__int64 delta = g_end_ticks - g_begin_ticks;
printf("Speed %f Mb per sec\n", (g_total_bytes_written * 1000.0) / (1024.0 * 1024 * delta));
char c;
scanf("%c", &c);
::CloseHandle(hFile);
return 0;
}
