diff --git a/CMakeLists.txt b/CMakeLists.txt
index 340ee74300393fbd32c6f922c1c6e921c7842ce7..94f4d1b837aabf391be043cfffacd0816c28c205 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -23,7 +23,7 @@ find_package(catkin REQUIRED COMPONENTS
 # find_package(Boost REQUIRED COMPONENTS system)
 # find_package(Ceres REQUIRED)
 # find_package(Eigen3 REQUIRED)
-find_package(wolf REQUIRED)
+find_package(wolfcore REQUIRED)
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
@@ -122,11 +122,11 @@ INCLUDE_DIRS include
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-message("Wolf include path: ${wolf_INCLUDE_DIR}")
+message("Wolf include path: ${wolf_INCLUDE_DIRS}")
 include_directories(
     include
   ${EIGEN_INCLUDE_DIRS}
-  ${wolf_INCLUDE_DIRS}
+  ${wolfcore_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
   ${CERES_INCLUDE_DIRS}
 )
@@ -171,7 +171,7 @@ add_dependencies(visualizer ${PROJECT_NAME}_gencfg)
 target_link_libraries(subscriber_${PROJECT_NAME}
                       ${catkin_LIBRARIES}
                       ${CERES_LIBRARIES}
-                      ${wolf_LIBRARIES}
+                      ${wolfcore_LIBRARIES}
                       yaml-cpp
                       dl
                       )
@@ -179,7 +179,7 @@ target_link_libraries(subscriber_${PROJECT_NAME}
 target_link_libraries(publisher_${PROJECT_NAME}
                       ${catkin_LIBRARIES}
                       ${CERES_LIBRARIES}
-                      ${wolf_LIBRARIES}
+                      ${wolfcore_LIBRARIES}
                       yaml-cpp
                       dl
                       )
@@ -188,7 +188,7 @@ target_link_libraries(${PROJECT_NAME}
                       visualizer
                       ${catkin_LIBRARIES}
                       ${CERES_LIBRARIES}
-                      ${wolf_LIBRARIES}
+                      ${wolfcore_LIBRARIES}
                       yaml-cpp
                       dl
                       )
diff --git a/src/node.cpp b/src/node.cpp
index ff373d520b5600a836c04c1fe786554213927b9a..ee1238505321f4fbdf6e865b5e1a8ebd4270001d 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -65,22 +65,14 @@ WolfRosNode::WolfRosNode()
 
     viz_ = std::make_shared<Visualizer>();
     viz_->initialize(nh_);
-    viz_period_ = server.getParam<int>("visualizer/period");
+    viz_period_ = server.getParam<double>("visualizer/period");
 
     // ROS PUBLISHERS
     ROS_INFO("Creating publishers...");
-    try
+    for (auto it : server.getParam<std::vector<std::map<std::string, std::string>>>("ROS publisher"))
     {
-        for (auto it : server.getParam<std::vector<std::map<std::string, std::string>>>("ROS publisher"))
-        {
-            WOLF_INFO("Pub: ", it["type"]);
-            publishers_.push_back(FactoryPublisher::create(it["type"], it["type"]+it["topic"], server, problem_ptr_, nh_));
-        }
-    }
-    catch (MissingValueException& e)
-    {
-        WOLF_WARN(e.what());
-        WOLF_WARN("No publishers found...");
+        WOLF_INFO("Pub: ", it["type"]);
+        publishers_.push_back(FactoryPublisher::create(it["type"], it["type"]+it["topic"], server, problem_ptr_, nh_));
     }
 
     // TF INIT
@@ -97,6 +89,7 @@ void WolfRosNode::solve()
         ROS_INFO("================ solve ==================");
 
     std::string report = solver_->solve();
+
     if (!report.empty())
     {
         std::cout << report << std::endl;
@@ -123,7 +116,7 @@ void WolfRosNode::visualize()
     viz_->visualize(problem_ptr_);
     auto stop     = std::chrono::high_resolution_clock::now();
     auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
-    //std::cout << "Visualize took " << duration.count() << " microseconds" << std::endl;
+    std::cout << "Visualize took " << duration.count() << " microseconds" << std::endl;
 }
 
 bool WolfRosNode::updateTf()
@@ -248,6 +241,7 @@ int main(int argc, char **argv)
         auto start3 = std::chrono::high_resolution_clock::now();
         if ((ros::Time::now() - last_viz_time).toSec() >= wolf_node.viz_period_)
         {
+            std::cout << "Last Viz since/viz_period_ " << (ros::Time::now() - last_viz_time).toSec() << " / " << wolf_node.viz_period_ << std::endl;
             wolf_node.visualize();
             last_viz_time = ros::Time::now();
         }
diff --git a/src/subscriber_diffdrive.cpp b/src/subscriber_diffdrive.cpp
index 7c30b79726a903554999e967e482de8c983b108c..b403a7c8f233370b05d082943a30fe05ad89d448 100644
--- a/src/subscriber_diffdrive.cpp
+++ b/src/subscriber_diffdrive.cpp
@@ -43,7 +43,7 @@ void SubscriberDiffdrive::callback(const sensor_msgs::JointState::ConstPtr& msg)
           TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), sensor_ptr_, angles_inc, cov, nullptr);
       sensor_ptr_->process(cptr);
 
-      auto current_kf = sensor_ptr_->getProblem()->getLastKeyFrame()->id();
+      auto current_kf = sensor_ptr_->getProblem()->getLastFrame()->id();
 
       if(last_kf != current_kf)
       {
diff --git a/src/visualizer.cpp b/src/visualizer.cpp
index 276bc6633f30579f2e31e33a27578c892be681d1..79d5739c99751bd99832104740a7f078b2a8d40b 100644
--- a/src/visualizer.cpp
+++ b/src/visualizer.cpp
@@ -22,6 +22,7 @@ void Visualizer::initialize(ros::NodeHandle& nh)
     nh.param<double>(       "factors_width",            factors_width_,             0.02);
     nh.param<double>(       "factors_absolute_height",  factors_absolute_height_,   20);
     nh.param<double>(       "landmark_text_z_offset",   landmark_text_z_offset_,    1);
+    nh.param<double>(       "landmark_width",           landmark_width_,            0.1);
     nh.param<double>(       "landmark_length",          landmark_length_,           1);
     nh.param<double>(       "frame_width",              frame_width_,               0.1);
     nh.param<double>(       "frame_length",             frame_length_,              1);
@@ -242,7 +243,7 @@ void Visualizer::publishTrajectory(const ProblemPtr problem)
     int marker_i = 0;
     auto frame_marker = frame_marker_;
     auto frame_text_marker = frame_text_marker_;
-    for (auto frm : problem->getTrajectory()->getFrameList())
+    for (auto frm : *problem->getTrajectory())
         if (frm->isKey())
         {
             // fill marker