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; }