AnimationController* _3dsLoader::loadFromFile(const string &fileName) const { PropertyBag xml; bool truespaceModel = false; string skin; TRACE(string("Loading 3DS model from \"") + fileName + string("\"")); xml.loadFromFile(fileName); AnimationController* controller = new AnimationController(); xml.get_optional("Truespace", truespaceModel); xml.get_optional("forceSkin", skin); for(size_t i=0, numAnimations=xml.count("animation"); i<numAnimations; ++i) { PropertyBag animation; string name; float fps = 0.0; bool looping = false; float priority = 0.0f; xml.get("animation", animation, i); animation.get("name", name); animation.get_optional("fps", fps); animation.get_optional("looping", looping); animation.get_optional("priority", priority); // Load all the keyframes vector<KeyFrame> keyFrames; const size_t length=animation.count("keyframe"); for(size_t j=0; j<length; ++j) { string keyFrameFile; animation.get("keyframe", keyFrameFile, j); Model keyFrame = loadKeyFrame(keyFrameFile); cullDegenerateMeshes(keyFrame); if(!skin.empty()) forceSkin(keyFrame, skin); if(truespaceModel) fixTrueSpaceVertices(keyFrame); keyFrames.push_back(keyFrame); } TRACE(string("Adding 3DS animation \"") + name + string("\" from ") + fileName); // Add it to the controller AnimationSequence animationSequence(keyFrames, name, priority, looping, 0, length, fps); controller->addAnimation(animationSequence); } return controller; }
void updateEditor(SDL_Event event, EditorData* data) { GUI_sendEventToGUI(data->gui, &event); const unsigned char* keyboardState = SDL_GetKeyboardState(NULL); int2 mousePixel; SDL_GetMouseState(&mousePixel.x, &mousePixel.y); float2 mousePos = pixelToPoint(&data->view, mousePixel); updateBones(data); switch (event.type) { case SDL_KEYDOWN: { switch (event.key.keysym.scancode) { case SDL_SCANCODE_S: if (event.key.keysym.mod == KMOD_LCTRL) { SDL_Event lastEvent = data->lowEventStack.Top(); if (lastEvent.key.keysym.scancode == SDL_SCANCODE_S && lastEvent.key.keysym.mod == KMOD_LCTRL) { // save skeleton GUI_WidgetID widget = GUI_addContainer(data->gui, GUI_MAIN_WIDGET, { 400 - 100, 300 - 25, 200, 50}, GUI_Container::Orientation::HORIZONTAL); auto callbackSave = [](GUI* gui, GUI_WidgetID widgetID, void* editorData) { EditorData* data = (EditorData*)editorData; GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID); const char* filename = GUI_getTextfield(gui, GUI_getChildWidget(gui, widget, 0)).text; saveSkeleton(filename, &data->skeleton[0], &data->names[0], data->skeleton.size()); GUI_removeWidget(gui, widget); }; auto callbackCancel = [](GUI* gui, GUI_WidgetID widgetID, void*) { GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID); GUI_removeWidget(gui, widget); }; GUI_addTextfield(data->gui, widget, { 0, 0, -1, -1 }, "filename"); GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Save", data, callbackSave); GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Cancel", 0, callbackCancel); break; } else if (lastEvent.key.keysym.scancode == SDL_SCANCODE_O && lastEvent.key.keysym.mod == KMOD_LCTRL) { // load skeleton GUI_WidgetID widget = GUI_addContainer(data->gui, GUI_MAIN_WIDGET, { 400 - 100, 300 - 15, 200, 30 }, GUI_Container::Orientation::HORIZONTAL); auto callbackLoad = [](GUI* gui, GUI_WidgetID widgetID, void* editorData) { EditorData* data = (EditorData*)editorData; GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID); const char* filename = GUI_getTextfield(gui, GUI_getChildWidget(gui, widget, 0)).text; strcpy(data->skeletonName, filename); const SkeletonSave* save = loadSkeleton(filename); if (save != 0) { data->bones.clear(); data->skeleton.clear(); data->constraints.clear(); data->names.clear(); const char* name = save->names; for (int i = 0; i < save->numBones; ++i) { Bone newBone; newBone.jointAngle = { 1, 0 }; newBone.jointPos = save->jointPos[i]; newBone.parentJoint = save->parentJoints[i]; newBone.name = data->names.push({ { 0 } }); memcpy(data->names[newBone.name].string, name, save->nameLen[i] * sizeof(char)); name += save->nameLen[i]; Constraint constraint; constraint.minAngle = -PI; constraint.maxAngle = 3 * PI; data->skeleton.push(newBone); data->constraints.push(constraint); } updateBones(data); } GUI_removeWidget(gui, widget); }; auto callbackCancel = [](GUI* gui, GUI_WidgetID widgetID, void*) { GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID); GUI_removeWidget(gui, widget); }; GUI_addTextfield(data->gui, widget, { 0, 0, -1, -1 }, "filename"); GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Load", data, callbackLoad); GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Cancel", 0, callbackCancel); break; } data->lowEventStack.Push(event); } break; case SDL_SCANCODE_K: if (event.key.keysym.mod == KMOD_LCTRL) { SDL_Event lastEvent = data->lowEventStack.Top(); if (lastEvent.key.keysym.scancode == SDL_SCANCODE_S && lastEvent.key.keysym.mod == KMOD_LCTRL) { // save keyFrame GUI_WidgetID widget = GUI_addContainer(data->gui, GUI_MAIN_WIDGET, { 400 - 100, 300 - 15, 200, 30 }, GUI_Container::Orientation::HORIZONTAL); auto callbackSave = [](GUI* gui, GUI_WidgetID widgetID, void* editorData) { EditorData* data = (EditorData*)editorData; GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID); const char* filename = GUI_getTextfield(gui, GUI_getChildWidget(gui, widget, 0)).text; saveKeyFrame(filename, &data->skeleton[0], &data->names[0], data->skeleton.size()); GUI_removeWidget(gui, widget); }; auto callbackCancel = [](GUI* gui, GUI_WidgetID widgetID, void*) { GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID); GUI_removeWidget(gui, widget); }; GUI_addTextfield(data->gui, widget, { 0, 0, -1, -1 }, "filename"); GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Save", data, callbackSave); GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Cancel", 0, callbackCancel); break; } else if (lastEvent.key.keysym.scancode == SDL_SCANCODE_O && lastEvent.key.keysym.mod == KMOD_LCTRL) { // load keyframe GUI_WidgetID widget = GUI_addContainer(data->gui, GUI_MAIN_WIDGET, { 400 - 100, 300 - 15, 200, 30 }, GUI_Container::Orientation::HORIZONTAL); auto callbackLoad = [](GUI* gui, GUI_WidgetID widgetID, void* editorData) { EditorData* data = (EditorData*)editorData; GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID); const char* filename = GUI_getTextfield(gui, GUI_getChildWidget(gui, widget, 0)).text; strcpy(data->frameName, filename); const KeyFrameSave* save = loadKeyFrame(filename); if (save != 0) { const char* name = save->names; for (int i = 0; i < save->numBones; ++i) { int nameID = -1; for (int j = 0; j < data->names.size(); ++j) { if (strncmp(name, data->names[j].string, save->nameLen[i]) == 0) { nameID = j; break; } } if (nameID != -1) { if (data->skeleton[nameID].name == nameID) { data->skeleton[nameID].jointAngle = save->jointAngles[i]; } else { for (int j = 0; j < data->skeleton.size(); j++) { if (data->skeleton[j].name = nameID) data->skeleton[j].jointAngle = save->jointAngles[i]; } } } name += save->nameLen[i]; } updateBones(data); } GUI_removeWidget(gui, widget); }; auto callbackCancel = [](GUI* gui, GUI_WidgetID widgetID, void*) { GUI_WidgetID widget = GUI_getParentWidget(gui, widgetID); GUI_removeWidget(gui, widget); }; GUI_addTextfield(data->gui, widget, { 0, 0, -1, -1 }, "filename"); GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Load", data, callbackLoad); GUI_addButton(data->gui, widget, { 0, 0, 50, -1 }, "Cancel", 0, callbackCancel); break; } data->lowEventStack.Push(event); } break; default: data->lowEventStack.Push(event); break; } break; } case SDL_MOUSEBUTTONDOWN: { data->newBone = -1; if (keyboardState[SDL_SCANCODE_A]) { for (int i = 0; i < data->bones.size(); ++i) { if (pointInRect(mousePos, data->bones[i].rect)) { int name = data->names.push({ "test" }); Bone newBone; newBone.jointAngle = { 1, 0 }; newBone.jointPos = { 0, 0 }; newBone.parentJoint = i; newBone.name = name; Constraint constraint; constraint.minAngle = -PI; constraint.maxAngle = 3 * PI; int newBoneID = data->skeleton.push(newBone); data->constraints.push(constraint); int* ids = new int[data->skeleton.size()]; sortSkeleton(&data->skeleton[0], data->skeleton.size(), ids); Constraint* tempConstraints = new Constraint[data->constraints.size()]; memcpy(tempConstraints, &data->constraints[0], sizeof(Constraint) * data->constraints.size()); for (int i = 0; i < data->constraints.size(); ++i) { data->constraints[i] = tempConstraints[ids[i]]; } delete[] tempConstraints; //find bone again data->newBone = ids[newBoneID]; // rebuild bone selection updateBones(data); delete[]ids; break; } } } else if (keyboardState[SDL_SCANCODE_N]) { for (int i = 0; i < data->bones.size(); ++i) { if (pointInRect(mousePos, data->bones[i].rect)) { int2 pos = pointToPixel(&data->view, float2{ data->bones[i].rect.x + 10, data->bones[i].rect.y }); GUI_WidgetID widget = GUI_addContainer(data->gui, GUI_MAIN_WIDGET, intRect{ pos.x, pos.y, 100, 30 }, GUI_Container::HORIZONTAL); struct Args { EditorData* data; int boneId; }*args = new Args{ data, i }; auto callbackOk = [](GUI* gui, GUI_WidgetID widgetID, void* args) { EditorData* data = ((Args*)args)->data; int boneID = ((Args*)args)->boneId; GUI_WidgetID parentID = GUI_getParentWidget(gui, widgetID); const char* newName = GUI_getTextfield(gui, GUI_getChildWidget(gui, parentID, 0)).text; strcpy(data->names[data->skeleton[boneID].name].string, newName); GUI_removeWidget(gui, parentID); delete args; }; GUI_addTextfield(data->gui, widget, intRect{ 0, 0, -1, -1 }, data->names[data->skeleton[i].name].string); GUI_addButton(data->gui, widget, intRect{ 0, 0, 30, -1 }, "OK", args, callbackOk); break; } } } else { for (int i = 0; i < data->bones.size(); i++) { if (pointInRect(mousePos, data->bones[i].rect)) { data->selectedBones.push(i); data->grabbedBone = i; } } if (data->grabbedBone == -1 && !keyboardState[SDL_SCANCODE_LCTRL]) data->selectedBones.clear(); } break; } case SDL_MOUSEBUTTONUP: { data->grabbedBone = -1; break; } case SDL_MOUSEMOTION: { if (SDL_GetMouseState(0, 0) == SDL_BUTTON_MIDDLE) { data->view.position.x -= (event.motion.xrel / data->view.scale.x); data->view.position.y -= (event.motion.yrel / data->view.scale.y); } else { float2 motion = rotate({ (float)event.motion.xrel, (float)event.motion.yrel }, data->view.angle) * float2{ 1 / data->view.scale.x, 1 / data->view.scale.y }; if (data->newBone != -1) { float2 angle = getBoneWorldTransform(&data->skeleton[0], data->skeleton.size(), data->newBone).angle; float2 rel = rotate(float2{ motion.x, motion.y }, negateRotation(angle)); data->skeleton[data->newBone].jointPos += rel; } if (data->grabbedBone != -1) { if (keyboardState[SDL_SCANCODE_LCTRL]) { float2 angle = getBoneWorldTransform(&data->skeleton[0], data->skeleton.size(), data->skeleton[data->grabbedBone].parentJoint).angle; float2 rel = rotate(float2{ motion.x, motion.y }, negateRotation(angle)); data->skeleton[data->grabbedBone].jointPos += rel; } else { animateCCDIKSelection(&data->skeleton[0], &data->constraints[0], data->skeleton.size(), &data->selectedBones[0], data->selectedBones.size(), data->grabbedBone, mousePos - float2{ 400, 300 }); } } } break; } case SDL_MOUSEWHEEL: { data->view.scale.x += event.wheel.y*0.1f; data->view.scale.y += event.wheel.y*0.1f; break; } } }
void PoseGraph::loadPoseGraph() { TicToc tmp_t; FILE * pFile; string file_path = POSE_GRAPH_SAVE_PATH + "pose_graph.txt"; printf("lode pose graph from: %s \n", file_path.c_str()); printf("pose graph loading...\n"); pFile = fopen (file_path.c_str(),"r"); if (pFile == NULL) { printf("lode previous pose graph error: wrong previous pose graph path or no previous pose graph \n the system will start with new pose graph \n"); return; } int index; double time_stamp; double VIO_Tx, VIO_Ty, VIO_Tz; double PG_Tx, PG_Ty, PG_Tz; double VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz; double PG_Qw, PG_Qx, PG_Qy, PG_Qz; double loop_info_0, loop_info_1, loop_info_2, loop_info_3; double loop_info_4, loop_info_5, loop_info_6, loop_info_7; int loop_index; int keypoints_num; Eigen::Matrix<double, 8, 1 > loop_info; int cnt = 0; while (fscanf(pFile,"%d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d", &index, &time_stamp, &VIO_Tx, &VIO_Ty, &VIO_Tz, &PG_Tx, &PG_Ty, &PG_Tz, &VIO_Qw, &VIO_Qx, &VIO_Qy, &VIO_Qz, &PG_Qw, &PG_Qx, &PG_Qy, &PG_Qz, &loop_index, &loop_info_0, &loop_info_1, &loop_info_2, &loop_info_3, &loop_info_4, &loop_info_5, &loop_info_6, &loop_info_7, &keypoints_num) != EOF) { /* printf("I read: %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %d\n", index, time_stamp, VIO_Tx, VIO_Ty, VIO_Tz, PG_Tx, PG_Ty, PG_Tz, VIO_Qw, VIO_Qx, VIO_Qy, VIO_Qz, PG_Qw, PG_Qx, PG_Qy, PG_Qz, loop_index, loop_info_0, loop_info_1, loop_info_2, loop_info_3, loop_info_4, loop_info_5, loop_info_6, loop_info_7, keypoints_num); */ cv::Mat image; std::string image_path, descriptor_path; if (DEBUG_IMAGE) { image_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_image.png"; image = cv::imread(image_path.c_str(), 0); } Vector3d VIO_T(VIO_Tx, VIO_Ty, VIO_Tz); Vector3d PG_T(PG_Tx, PG_Ty, PG_Tz); Quaterniond VIO_Q; VIO_Q.w() = VIO_Qw; VIO_Q.x() = VIO_Qx; VIO_Q.y() = VIO_Qy; VIO_Q.z() = VIO_Qz; Quaterniond PG_Q; PG_Q.w() = PG_Qw; PG_Q.x() = PG_Qx; PG_Q.y() = PG_Qy; PG_Q.z() = PG_Qz; Matrix3d VIO_R, PG_R; VIO_R = VIO_Q.toRotationMatrix(); PG_R = PG_Q.toRotationMatrix(); Eigen::Matrix<double, 8, 1 > loop_info; loop_info << loop_info_0, loop_info_1, loop_info_2, loop_info_3, loop_info_4, loop_info_5, loop_info_6, loop_info_7; if (loop_index != -1) if (earliest_loop_index > loop_index || earliest_loop_index == -1) { earliest_loop_index = loop_index; } // load keypoints, brief_descriptors string brief_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_briefdes.dat"; std::ifstream brief_file(brief_path, std::ios::binary); string keypoints_path = POSE_GRAPH_SAVE_PATH + to_string(index) + "_keypoints.txt"; FILE *keypoints_file; keypoints_file = fopen(keypoints_path.c_str(), "r"); vector<cv::KeyPoint> keypoints; vector<cv::KeyPoint> keypoints_norm; vector<BRIEF::bitset> brief_descriptors; for (int i = 0; i < keypoints_num; i++) { BRIEF::bitset tmp_des; brief_file >> tmp_des; brief_descriptors.push_back(tmp_des); cv::KeyPoint tmp_keypoint; cv::KeyPoint tmp_keypoint_norm; double p_x, p_y, p_x_norm, p_y_norm; if(!fscanf(keypoints_file,"%lf %lf %lf %lf", &p_x, &p_y, &p_x_norm, &p_y_norm)) printf(" fail to load pose graph \n"); tmp_keypoint.pt.x = p_x; tmp_keypoint.pt.y = p_y; tmp_keypoint_norm.pt.x = p_x_norm; tmp_keypoint_norm.pt.y = p_y_norm; keypoints.push_back(tmp_keypoint); keypoints_norm.push_back(tmp_keypoint_norm); } brief_file.close(); fclose(keypoints_file); KeyFrame* keyframe = new KeyFrame(time_stamp, index, VIO_T, VIO_R, PG_T, PG_R, image, loop_index, loop_info, keypoints, keypoints_norm, brief_descriptors); loadKeyFrame(keyframe, 0); if (cnt % 20 == 0) { publish(); } cnt++; } fclose (pFile); printf("load pose graph time: %f s\n", tmp_t.toc()/1000); base_sequence = 0; }