diff --git a/config/navigation_jackal_classifier.lua b/config/navigation_jackal_classifier.lua
index 13930eb..e62ca1d 100644
--- a/config/navigation_jackal_classifier.lua
+++ b/config/navigation_jackal_classifier.lua
@@ -17,7 +17,7 @@ NavigationParameters = {
   max_linear_accel = 0.5;
   max_linear_decel = 0.5;
   -- max_linear_speed = 1.4;
-  max_linear_speed = 1.5;
+  max_linear_speed = .8;
   max_angular_accel = 0.5;
   max_angular_decel = 0.5;
   max_angular_speed = 1.0;
@@ -63,9 +63,9 @@ TerrainEvaluator = {
   model_path = "../terrain_models/classifier_model.pt";
 
   -- dist_to_goal_weight = -0.2;
-  -- dist_to_goal_weight = -0.7;
+  dist_to_goal_weight = -0.7;
   -- dist_to_goal_weight = -2.0;
-  dist_to_goal_weight = 0;
+  --  dist_to_goal_weight = 0;
 
   clearance_weight = 0; -- -0.25;
   fpl_weight = 0; -- -0.75;
diff --git a/config/navigation_jackal_icl.lua b/config/navigation_jackal_icl.lua
index bc7f099..9f39b15 100644
--- a/config/navigation_jackal_icl.lua
+++ b/config/navigation_jackal_icl.lua
@@ -3,37 +3,38 @@ function deg2rad(deg)
 end
 
 NavigationParameters = {
-  laser_topic = "velodyne_2dscan_highbeams";
+  laser_topic = "/velodyne_2dscan_lowbeam";
   odom_topic = "/odometry/filtered";
   localization_topic = "localization";
   image_topic = "/bev/single/compressed";
   init_topic = "initialpose";
   enable_topic = "autonomy_arbiter/enabled";
   laser_loc = {
-    x = 0.065;
-    y = 0;
+    x = 0.095;
+    y = 0.1;
   };
   dt = 0.025;
-  max_linear_accel = 0.5;
-  max_linear_decel = 0.5;
+  max_linear_accel = 0.3;
+  max_linear_decel = 0.3;
   -- max_linear_speed = 1.4;
-  max_linear_speed = 1.5;
+  max_linear_speed = 1.0;
   max_angular_accel = 0.5;
-  max_angular_decel = 0.5;
+  max_angular_decel = 0.3; -- 0.5
   max_angular_speed = 1.0;
   carrot_dist = 250;  -- large carrot distance so the goal does not latch onto the map
-  system_latency = 0.24;
-  obstacle_margin = 0.15;
-  num_options = 31;
+  system_latency = 0.24; -- 0.24
+  obstacle_margin = 0.2; -- 0.15
+  num_options = 21;
   -- num_options = 15;
-  robot_width = 0.44;
-  robot_length = 0.5;
+  robot_width = 0.2;
+  robot_length = 0.4;
   base_link_offset = 0;
-  max_free_path_length = 4.0;
+  max_free_path_length = 7.5;
   -- max_free_path_length = 3.5;  -- ahg demo
   -- max_free_path_length = 1;  -- ahg demo
   max_clearance = 1.0;
   can_traverse_stairs = false;
+  -- evaluator_type = "linear";
   evaluator_type = "terrain2";
   camera_calibration_path = "config/camera_calibration_kinect.yaml";
   model_path = "";
@@ -46,29 +47,28 @@ NavigationParameters = {
 };
 
 AckermannSampler = {
-  max_curvature = 2.5;
+  max_curvature = 0.5;
   -- max_curvature = 5;
   clearance_path_clip_fraction = 0.8;
 };
 
+Context = {
+  context_path="../terrain_models/realistic_context.pt";
+}
+
 TerrainEvaluator = {
   patch_size_pixels = 1;
   bev_pixels_per_meter = 150;
   min_cost = 0.0;
-  max_cost = 1;
+  max_cost = 1.0;
   discount_factor = 0.9;
   -- discount_factor = 0.8; -- ahg demo
   rollout_density = 20;
 
   model_path = "../terrain_models/model.pt";
-  context_path="../terrain_models/context.pt";
-
-  -- dist_to_goal_weight = -0.2;
-  -- dist_to_goal_weight = -0.7;
-  -- dist_to_goal_weight = -2.0;
-  dist_to_goal_weight = 0;
 
-  clearance_weight = 0; -- -0.25;
-  fpl_weight = 0; -- -0.75;
-  terrain_weight = 4.0;
+  dist_to_goal_weight = 0.0;
+  clearance_weight = -0.25; -- [0, max_clearance] 
+  fpl_weight = -0.1; -- [0, max_free_path_length] -> [0, -0.75]
+  terrain_weight = 0.0; -- [0, 1] Usually <1 due to discounting factor + norm cost
 }
diff --git a/config/navigation_jackal_sterling.lua b/config/navigation_jackal_sterling.lua
index 0716b35..65a3131 100644
--- a/config/navigation_jackal_sterling.lua
+++ b/config/navigation_jackal_sterling.lua
@@ -17,7 +17,7 @@ NavigationParameters = {
   max_linear_accel = 0.5;
   max_linear_decel = 0.5;
   -- max_linear_speed = 1.4;
-  max_linear_speed = 1.5;
+  max_linear_speed = 0.8;
   max_angular_accel = 0.5;
   max_angular_decel = 0.5;
   max_angular_speed = 1.0;
@@ -63,9 +63,9 @@ TerrainEvaluator = {
   model_path = "../terrain_models/sterling_model.pt";
 
   -- dist_to_goal_weight = -0.2;
-  -- dist_to_goal_weight = -0.7;
+  dist_to_goal_weight = -0.7;
   -- dist_to_goal_weight = -2.0;
-  dist_to_goal_weight = 0;
+  --  dist_to_goal_weight = 0;
 
   clearance_weight = 0; -- -0.25;
   fpl_weight = 0; -- -0.75;
diff --git a/src/navigation/ackermann_motion_primitives.cc b/src/navigation/ackermann_motion_primitives.cc
index d57103c..61b0b1e 100644
--- a/src/navigation/ackermann_motion_primitives.cc
+++ b/src/navigation/ackermann_motion_primitives.cc
@@ -19,26 +19,26 @@
 */
 //========================================================================
 
+#include "ackermann_motion_primitives.h"
+
 #include <math.h>
 
 #include <algorithm>
 #include <memory>
 #include <vector>
 
-#include "math/poses_2d.h"
+#include "config_reader/config_reader.h"
 #include "eigen3/Eigen/Dense"
 #include "eigen3/Eigen/Geometry"
-#include "config_reader/config_reader.h"
+#include "math/poses_2d.h"
 #include "motion_primitives.h"
-#include "constant_curvature_arcs.h"
-#include "ackermann_motion_primitives.h"
 
-using std::min;
+using Eigen::Vector2f;
+using pose_2d::Pose2Df;
 using std::max;
-using std::vector;
+using std::min;
 using std::shared_ptr;
-using pose_2d::Pose2Df;
-using Eigen::Vector2f;
+using std::vector;
 using namespace math_util;
 
 CONFIG_FLOAT(max_curvature, "AckermannSampler.max_curvature");
@@ -51,8 +51,7 @@ const float kEpsilon = 1e-5;
 
 namespace motion_primitives {
 
-AckermannSampler::AckermannSampler() {
-}
+AckermannSampler::AckermannSampler() {}
 
 void AckermannSampler::SetMaxPathLength(ConstantCurvatureArc* path_ptr) {
   ConstantCurvatureArc& path = *path_ptr;
@@ -63,29 +62,26 @@ void AckermannSampler::SetMaxPathLength(ConstantCurvatureArc* path_ptr) {
     // path.length = min(nav_params.max_free_path_length, local_target.x());
     path.fpl = path.length;
     return;
-  } 
+  }
   const float turn_radius = 1.0f / path.curvature;
   const float quarter_circle_dist = fabs(turn_radius) * M_PI_2;
   const Vector2f turn_center(0, turn_radius);
   const Vector2f target_radial = local_target - turn_center;
-  const Vector2f middle_radial =
-      fabs(turn_radius) * target_radial.normalized();
+  const Vector2f middle_radial = fabs(turn_radius) * target_radial.normalized();
   const float middle_angle =
       atan2(fabs(middle_radial.x()), fabs(middle_radial.y()));
   const float dist_closest_to_goal = middle_angle * fabs(turn_radius);
-  path.fpl = min<float>({
-      nav_params.max_free_path_length, 
-      quarter_circle_dist});
+  path.fpl = min<float>({nav_params.max_free_path_length, quarter_circle_dist});
 
   // eyang/terrain: for terrain-based navigation, do not restrict
-  // the path length to only make progress towards the goal 
+  // the path length to only make progress towards the goal
   path.length = path.fpl;
   std::ignore = dist_closest_to_goal;  // silence compiler warning
   // path.length = min<float>({
   //   path.fpl,
   //   dist_closest_to_goal
   // });
-  const float stopping_dist = 
+  const float stopping_dist =
       Sq(vel.x()) / (2.0 * nav_params.linear_limits.max_deceleration);
   // eyang/terrain: since the robot is now allowed to move past the goal,
   // sometimes the stopping distance makes the curve too long.
@@ -97,29 +93,27 @@ vector<shared_ptr<PathRolloutBase>> AckermannSampler::GetSamples(int n) {
   vector<shared_ptr<PathRolloutBase>> samples;
   if (false) {
     samples = {
-      shared_ptr<PathRolloutBase>(new ConstantCurvatureArc(-0.1)),
-      shared_ptr<PathRolloutBase>(new ConstantCurvatureArc(0)),
-      shared_ptr<PathRolloutBase>(new ConstantCurvatureArc(0.1)),
+        shared_ptr<PathRolloutBase>(new ConstantCurvatureArc(-0.1)),
+        shared_ptr<PathRolloutBase>(new ConstantCurvatureArc(0)),
+        shared_ptr<PathRolloutBase>(new ConstantCurvatureArc(0.1)),
     };
     return samples;
   }
-  const float max_domega = 
+  const float max_domega =
       nav_params.dt * nav_params.angular_limits.max_acceleration;
-  const float max_dv = 
+  const float max_dv =
       nav_params.dt * nav_params.linear_limits.max_acceleration;
   const float robot_speed = fabs(vel.x());
   float c_min = -CONFIG_max_curvature;
   float c_max = CONFIG_max_curvature;
   if (robot_speed > max_dv + kEpsilon) {
-    c_min = max<float>(
-        c_min, (ang_vel - max_domega) / (robot_speed - max_dv));
-    c_max = min<float>(
-        c_max, (ang_vel + max_domega) / (robot_speed - max_dv));
+    c_min = max<float>(c_min, (ang_vel - max_domega) / (robot_speed - max_dv));
+    c_max = min<float>(c_max, (ang_vel + max_domega) / (robot_speed - max_dv));
   }
   const float dc = (c_max - c_min) / static_cast<float>(n - 1);
   // printf("Options: %6.2f : %6.2f : %6.2f\n", c_min, dc, c_max);
   if (false) {
-    for (float c = c_min; c <= c_max; c+= dc) {
+    for (float c = c_min; c <= c_max; c += dc) {
       auto sample = new ConstantCurvatureArc(c);
       SetMaxPathLength(sample);
       CheckObstacles(sample);
@@ -128,7 +122,7 @@ vector<shared_ptr<PathRolloutBase>> AckermannSampler::GetSamples(int n) {
     }
   } else {
     const float dc = (2.0f * CONFIG_max_curvature) / static_cast<float>(n - 1);
-    for (float c = -CONFIG_max_curvature; c <= CONFIG_max_curvature; c+= dc) {
+    for (float c = -CONFIG_max_curvature; c <= CONFIG_max_curvature; c += dc) {
       auto sample = new ConstantCurvatureArc(c);
       SetMaxPathLength(sample);
       CheckObstacles(sample);
@@ -136,14 +130,15 @@ vector<shared_ptr<PathRolloutBase>> AckermannSampler::GetSamples(int n) {
       samples.push_back(shared_ptr<PathRolloutBase>(sample));
     }
   }
-  
+
   return samples;
 }
 
 void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
   ConstantCurvatureArc& path = *path_ptr;
   // How much the robot's body extends in front of its base link frame.
-  const float l = 0.5 * nav_params.robot_length - nav_params.base_link_offset + nav_params.obstacle_margin;
+  const float l = 0.5 * nav_params.robot_length - nav_params.base_link_offset +
+                  nav_params.obstacle_margin;
   // The robot's half-width.
   const float w = 0.5 * nav_params.robot_width + nav_params.obstacle_margin;
   if (fabs(path.curvature) < kEpsilon) {
@@ -160,16 +155,16 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
     path.fpl = max(0.0f, path.fpl);
     path.length = min(path.fpl, path.length);
 
-    const float stopping_dist = 
-      vel.squaredNorm() / (2.0 * nav_params.linear_limits.max_deceleration);
+    const float stopping_dist =
+        vel.squaredNorm() / (2.0 * nav_params.linear_limits.max_deceleration);
     if (path.fpl < stopping_dist) {
       path.length = 0;
     }
-    
+
     return;
   }
   const float path_radius = 1.0 / path.curvature;
-  const Vector2f c(0, path_radius );
+  const Vector2f c(0, path_radius);
   const float s = ((path_radius > 0.0) ? 1.0 : -1.0);
   const Vector2f inner_front_corner(l, s * w);
   const Vector2f outer_front_corner(l, -s * w);
@@ -179,11 +174,11 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
   const float r3_sq = (outer_front_corner - c).squaredNorm();
   float angle_min = M_PI;
   path.obstruction = Vector2f(-nav_params.max_free_path_length, 0);
-  // printf("%7.3f %7.3f %7.3f %7.3f\n", 
+  // printf("%7.3f %7.3f %7.3f %7.3f\n",
   //     path.curvature, sqrt(r1_sq), sqrt(r2_sq), sqrt(r3_sq));
   // The x-coordinate of the rear margin.
-  const float x_min = -0.5 * nav_params.robot_length + 
-      nav_params.base_link_offset - nav_params.obstacle_margin;
+  const float x_min = -0.5 * nav_params.robot_length +
+                      nav_params.base_link_offset - nav_params.obstacle_margin;
   using std::isfinite;
   for (const Vector2f& p : point_cloud) {
     if (!isfinite(p.x()) || !isfinite(p.y()) || p.x() < 0.0f) continue;
@@ -200,9 +195,9 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
     //        path.curvature, sqrt(r_sq), r1, sqrt(r2_sq), sqrt(r3_sq));
     if (r_sq < r1_sq || r_sq > r3_sq) continue;
     const float r = sqrt(r_sq);
-    const float theta = ((path.curvature > 0.0f) ?
-      atan2<float>(p.x(), path_radius - p.y()) :
-      atan2<float>(p.x(), p.y() - path_radius));
+    const float theta =
+        ((path.curvature > 0.0f) ? atan2<float>(p.x(), path_radius - p.y())
+                                 : atan2<float>(p.x(), p.y() - path_radius));
     float alpha;
     if (r_sq < r2_sq) {
       // Point will hit the side of the robot first.
@@ -232,18 +227,17 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
       angle_min = theta;
     }
   }
-  const float stopping_dist = 
+  const float stopping_dist =
       vel.squaredNorm() / (2.0 * nav_params.linear_limits.max_deceleration);
   if (path.length < stopping_dist) path.length = 0;
   path.length = max(0.0f, path.length);
   angle_min = min<float>(angle_min, path.length * fabs(path.curvature));
   path.clearance = nav_params.max_clearance;
 
-
   for (const Vector2f& p : point_cloud) {
-    const float theta = ((path.curvature > 0.0f) ?
-        atan2<float>(p.x(), path_radius - p.y()) :
-        atan2<float>(p.x(), p.y() - path_radius));
+    const float theta =
+        ((path.curvature > 0.0f) ? atan2<float>(p.x(), path_radius - p.y())
+                                 : atan2<float>(p.x(), p.y() - path_radius));
     if (theta < CONFIG_clearance_clip * angle_min && theta > 0.0) {
       const float r = (p - c).norm();
       const float current_clearance = fabs(r - fabs(path_radius));
@@ -253,7 +247,7 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
     }
   }
   path.clearance = max(0.0f, path.clearance);
-  // printf("%7.3f %7.3f %7.3f \n", 
+  // printf("%7.3f %7.3f %7.3f \n",
   //     path.curvature, path.length, path.clearance);
 }
 
diff --git a/src/navigation/ackermann_motion_primitives.h b/src/navigation/ackermann_motion_primitives.h
index 5dbba43..d253317 100644
--- a/src/navigation/ackermann_motion_primitives.h
+++ b/src/navigation/ackermann_motion_primitives.h
@@ -19,13 +19,12 @@
 */
 //========================================================================
 
-
 #include <memory>
 #include <vector>
 
-#include "math/poses_2d.h"
+#include "constant_curvature_arcs.h"
 #include "eigen3/Eigen/Dense"
-
+#include "math/poses_2d.h"
 #include "motion_primitives.h"
 
 #ifndef ACKERMANN_MOTION_PRIMITIVES_H
@@ -33,7 +32,7 @@
 
 namespace motion_primitives {
 
-// Path rollout sampler. 
+// Path rollout sampler.
 struct AckermannSampler : PathRolloutSamplerBase {
   // Given the robot's current dynamic state and an obstacle point cloud, return
   // a set of valid path rollout options that are collision-free.
@@ -42,15 +41,13 @@ struct AckermannSampler : PathRolloutSamplerBase {
   AckermannSampler();
 
   // Compute free path lengths and clearances.
-  void CheckObstacles(
-      std::vector<std::shared_ptr<PathRolloutBase>>& samples);
+  void CheckObstacles(std::vector<std::shared_ptr<PathRolloutBase>>& samples);
 
   // Limit the maximum path length to the closest point of approach to the local
   // target.
   void SetMaxPathLength(ConstantCurvatureArc* path);
 
   void CheckObstacles(ConstantCurvatureArc* path);
-
 };
 
 }  // namespace motion_primitives
diff --git a/src/navigation/astar.h b/src/navigation/astar.h
index d4e459a..32d1544 100644
--- a/src/navigation/astar.h
+++ b/src/navigation/astar.h
@@ -33,6 +33,7 @@
 
 // Project headers.
 #include "shared/math/math_util.h"
+#include "shared/util/timer.h"
 #include "simple_queue.h"
 
 #ifndef A_STAR_H
@@ -92,10 +93,8 @@ struct NodeHash {
 
 template <class Domain, class Visualizer>
 bool AStar(const typename Domain::State& start,
-           const typename Domain::State& goal,
-           const Domain& domain,
-           Visualizer* const viz,
-           std::vector<typename Domain::State>* path) {
+           const typename Domain::State& goal, const Domain& domain,
+           Visualizer* const viz, std::vector<typename Domain::State>* path) {
   static CumulativeFunctionTimer function_timer_(__FUNCTION__);
   CumulativeFunctionTimer::Invocation invoke(&function_timer_);
   static const uint64_t kMaxEdgeExpansions = 1000;
@@ -127,10 +126,8 @@ bool AStar(const typename Domain::State& start,
     // Get the node with the highest priority.
     const uint64_t k_current = queue.Pop();
     if (kDebug) {
-      printf("Add to closed: %5lu  g:%8.3f h:%8.3f\n",
-             k_current,
-             g_values_[k_current],
-             domain.Heuristic(k_current, k_goal));
+      printf("Add to closed: %5lu  g:%8.3f h:%8.3f\n", k_current,
+             g_values_[k_current], domain.Heuristic(k_current, k_goal));
     }
     closed_set_.insert(k_current);
     // Get all neighbors.
@@ -183,6 +180,5 @@ bool AStar(const typename Domain::State& start,
   return false;
 }
 
-
 }  // namespace navigation
 #endif  // A_STAR_H
diff --git a/src/navigation/linear_evaluator.cc b/src/navigation/linear_evaluator.cc
index b9809e1..2206069 100644
--- a/src/navigation/linear_evaluator.cc
+++ b/src/navigation/linear_evaluator.cc
@@ -19,44 +19,44 @@
 */
 //========================================================================
 
-#include <math.h>
+#include "linear_evaluator.h"
+
 #include <float.h>
+#include <math.h>
 
 #include <algorithm>
 #include <memory>
 #include <vector>
 
+#include "ackermann_motion_primitives.h"
+#include "constant_curvature_arcs.h"
+#include "eigen3/Eigen/Dense"
+#include "eigen3/Eigen/Geometry"
 #include "gflags/gflags.h"
 #include "math/line2d.h"
 #include "math/poses_2d.h"
-#include "eigen3/Eigen/Dense"
-#include "eigen3/Eigen/Geometry"
-
 #include "motion_primitives.h"
 #include "navigation_parameters.h"
-#include "constant_curvature_arcs.h"
-#include "ackermann_motion_primitives.h"
-#include "linear_evaluator.h"
 
-using std::min;
-using std::max;
-using std::vector;
-using std::shared_ptr;
-using pose_2d::Pose2Df;
 using Eigen::Vector2f;
 using navigation::MotionLimits;
+using pose_2d::Pose2Df;
+using std::max;
+using std::min;
+using std::shared_ptr;
+using std::vector;
 using namespace geometry;
 using namespace math_util;
 
 DEFINE_double(dw, 1, "Distance weight");
-DEFINE_double(cw, -0.5, "Clearance weight");
-DEFINE_double(fw, -1, "Free path weight");
+DEFINE_double(cw, -2, "Clearance weight");
+DEFINE_double(fw, -2, "Free path weight");
 DEFINE_double(subopt, 1.5, "Max path increase for clearance");
 
 namespace motion_primitives {
 
 shared_ptr<PathRolloutBase> LinearEvaluator::FindBest(
-    const vector<shared_ptr<PathRolloutBase>>& paths) {
+    const vector<shared_ptr<PathRolloutBase>> &paths) {
   if (paths.size() == 0) return nullptr;
 
   // Check if there is any path with an obstacle-free path from the end to the
@@ -66,8 +66,8 @@ shared_ptr<PathRolloutBase> LinearEvaluator::FindBest(
   bool path_to_goal_exists = false;
   for (size_t i = 0; i < paths.size(); ++i) {
     const auto endpoint = paths[i]->EndPoint().translation;
-    clearance_to_goal[i] = StraightLineClearance(
-        Line2f(endpoint, local_target), point_cloud);
+    clearance_to_goal[i] =
+        StraightLineClearance(Line2f(endpoint, local_target), point_cloud);
     if (clearance_to_goal[i] > 0.0) {
       dist_to_goal[i] = (endpoint - local_target).norm();
       path_to_goal_exists = true;
@@ -79,8 +79,9 @@ shared_ptr<PathRolloutBase> LinearEvaluator::FindBest(
   float best_path_length = FLT_MAX;
   for (size_t i = 0; i < paths.size(); ++i) {
     if (paths[i]->Length() <= 0.0f) continue;
-    const float path_length = (path_to_goal_exists ?
-        (paths[i]->Length() + dist_to_goal[i]) : dist_to_goal[i]);
+    const float path_length =
+        (path_to_goal_exists ? (paths[i]->Length() + dist_to_goal[i])
+                             : dist_to_goal[i]);
     if (path_length < best_path_length) {
       best_path_length = path_length;
       best = paths[i];
@@ -95,20 +96,26 @@ shared_ptr<PathRolloutBase> LinearEvaluator::FindBest(
 
   // Next try to find better paths.
   float best_cost = FLAGS_dw * (FLAGS_subopt * best_path_length) +
-      FLAGS_fw * best->Length() +
-      FLAGS_cw * best->Clearance();
+                    FLAGS_fw * best->Length() + FLAGS_cw * best->Clearance();
   for (size_t i = 0; i < paths.size(); ++i) {
     if (paths[i]->Length() <= 0.0f) continue;
-    const float path_length = (path_to_goal_exists ?
-        (paths[i]->Length() + dist_to_goal[i]) : dist_to_goal[i]);
-    const float cost = FLAGS_dw * path_length +
-      FLAGS_fw * paths[i]->Length() +
-      FLAGS_cw * paths[i]->Clearance();
+    const float path_length =
+        (path_to_goal_exists ? (paths[i]->Length() + dist_to_goal[i])
+                             : dist_to_goal[i]);
+    const float cost = FLAGS_dw * path_length + FLAGS_fw * paths[i]->Length() +
+                       FLAGS_cw * paths[i]->Clearance();
     if (cost < best_cost) {
       best = paths[i];
       best_cost = cost;
     }
   }
+
+  if (FLAGS_v > 1) {
+    printf("Best path: %f\n", best_cost);
+    printf("Best path length: %f\n", best->Length());
+    printf("Best path clearance: %f\n", best->Clearance());
+  }
+
   return best;
 }
 
diff --git a/src/navigation/motion_primitives.cc b/src/navigation/motion_primitives.cc
index db2f938..4384dbd 100644
--- a/src/navigation/motion_primitives.cc
+++ b/src/navigation/motion_primitives.cc
@@ -19,6 +19,8 @@
 */
 //========================================================================
 
+#include "motion_primitives.h"
+
 #include <float.h>
 #include <math.h>
 #include <stdio.h>
@@ -27,32 +29,27 @@
 #include <memory>
 #include <vector>
 
+#include "eigen3/Eigen/Dense"
 #include "math/line2d.h"
 #include "math/poses_2d.h"
-#include "eigen3/Eigen/Dense"
-
-#include "shared/math/math_util.h"
-#include "motion_primitives.h"
 #include "navigation_parameters.h"
+#include "shared/math/math_util.h"
 
 using Eigen::Vector2f;
+using navigation::MotionLimits;
 using std::max;
 using std::min;
 using std::shared_ptr;
 using std::string;
 using std::vector;
-using navigation::MotionLimits;
 using namespace math_util;
 using namespace geometry;
 
 namespace motion_primitives {
 
-float Run1DTimeOptimalControl(const MotionLimits& limits, 
-                              const float x_now, 
-                              const float v_now, 
-                              const float x_final, 
-                              const float v_final,
-                              const float dt) {
+float Run1DTimeOptimalControl(const MotionLimits& limits, const float x_now,
+                              const float v_now, const float x_final,
+                              const float v_final, const float dt) {
   // Non-zero final vel not yet implemented.
   CHECK_EQ(v_final, 0.0f) << "Non-zero final vel not yet implemented!";
   static FILE* fid = nullptr;
@@ -70,10 +67,9 @@ float Run1DTimeOptimalControl(const MotionLimits& limits,
       (speed + 0.5 * dv_a) * dt +
       Sq(speed + dv_a) / (2.0 * limits.max_deceleration);
   float cruise_stopping_dist =
-      speed * dt +
-      Sq(speed) / (2.0 * limits.max_deceleration);
+      speed * dt + Sq(speed) / (2.0 * limits.max_deceleration);
   char phase = '?';
-  if (dist_left >  0) {
+  if (dist_left > 0) {
     if (speed > limits.max_speed) {
       // Over max speed, slow down.
       phase = 'O';
@@ -96,35 +92,23 @@ float Run1DTimeOptimalControl(const MotionLimits& limits,
     velocity_cmd = max<float>(0, speed - dv_d);
   }
   if (kTest) {
-    printf("%c x:%f dist_left:%f a_dist:%f c_dist:%f v:%f cmd:%f\n",
-           phase,
-           x_now,
-           dist_left,
-           accel_stopping_dist,
-           cruise_stopping_dist,
-           v_now,
+    printf("%c x:%f dist_left:%f a_dist:%f c_dist:%f v:%f cmd:%f\n", phase,
+           x_now, dist_left, accel_stopping_dist, cruise_stopping_dist, v_now,
            velocity_cmd);
     if (fid != nullptr) {
-      fprintf(fid, "%f %f %f %f %f %f\n",
-              x_now,
-              dist_left,
-              accel_stopping_dist,
-              cruise_stopping_dist,
-              v_now,
-              velocity_cmd);
+      fprintf(fid, "%f %f %f %f %f %f\n", x_now, dist_left, accel_stopping_dist,
+              cruise_stopping_dist, v_now, velocity_cmd);
       fflush(fid);
     }
   }
   return velocity_cmd;
 }
 
-
-float StraightLineClearance(const Line2f& l, 
-                            const vector<Vector2f>& points) {
+float StraightLineClearance(const Line2f& l, const vector<Vector2f>& points) {
   const Vector2f d = l.Dir();
   const float len = l.Length();
   float clearance = FLT_MAX;
-  for (const Vector2f& p  : points) {
+  for (const Vector2f& p : points) {
     const float x = d.dot(p - l.p0);
     if (x < 0.0f || x > len) continue;
     clearance = min<float>(clearance, l.Distance(p));
diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
index d972321..dad7047 100644
--- a/src/navigation/navigation.cc
+++ b/src/navigation/navigation.cc
@@ -19,31 +19,30 @@
 */
 //========================================================================
 
+#include "navigation.h"
+
 #include <algorithm>
 #include <cmath>
 #include <deque>
 #include <memory>
 #include <string>
 
-#include "navigation.h"
-#include "geometry_msgs/PoseStamped.h"
-#include "gflags/gflags.h"
+#include "ackermann_motion_primitives.h"
+#include "astar.h"
+#include "constant_curvature_arcs.h"
+#include "deep_cost_map_evaluator.h"
 #include "eigen3/Eigen/Dense"
 #include "eigen3/Eigen/Geometry"
+#include "eight_connected_domain.h"
+#include "geometry_msgs/PoseStamped.h"
+#include "gflags/gflags.h"
 #include "glog/logging.h"
+#include "graph_domain.h"
+#include "linear_evaluator.h"
+#include "motion_primitives.h"
 #include "shared/math/math_util.h"
 #include "shared/util/helpers.h"
 #include "shared/util/timer.h"
-#include "shared/util/timer.h"
-#include "eight_connected_domain.h"
-#include "graph_domain.h"
-#include "astar.h"
-
-#include "motion_primitives.h"
-#include "constant_curvature_arcs.h"
-#include "ackermann_motion_primitives.h"
-#include "deep_cost_map_evaluator.h"
-#include "linear_evaluator.h"
 #include "terrain_evaluator.h"
 #include "terrain_evaluator2.h"
 
@@ -56,9 +55,9 @@ using std::atan2;
 using std::deque;
 using std::max;
 using std::min;
-using std::swap;
 using std::shared_ptr;
 using std::string;
+using std::swap;
 using std::vector;
 
 using namespace math_util;
@@ -86,19 +85,14 @@ DEFINE_int32(num_options, 41, "Number of options to consider");
 // TODO(jaholtz) figure out how to handle this visualization without
 // having astar contain ros dependencies
 struct EightGridVisualizer {
-  EightGridVisualizer(bool visualize) :
-             kVisualize(visualize) { }
+  EightGridVisualizer(bool visualize) : kVisualize(visualize) {}
 
   void DrawEdge(const navigation::EightConnectedDomain::State& s1,
                 const navigation::EightConnectedDomain::State& s2) {
     if (!kVisualize) return;
     static const bool kDebug = false;
     if (kDebug) {
-      printf("%7.2f,%7.2f -> %7.2f,%7.2f\n",
-            s1.x(),
-            s1.y(),
-            s2.x(),
-            s2.y());
+      printf("%7.2f,%7.2f -> %7.2f,%7.2f\n", s1.x(), s1.y(), s2.x(), s2.y());
     }
     // visualization::DrawLine(s1, s2, 0x606060, global_viz_msg_);
     // viz_pub_.publish(global_viz_msg_);
@@ -109,19 +103,15 @@ struct EightGridVisualizer {
 };
 
 struct GraphVisualizer {
-  GraphVisualizer(bool visualize) :
-             kVisualize(visualize) { }
+  GraphVisualizer(bool visualize) : kVisualize(visualize) {}
 
   void DrawEdge(const navigation::GraphDomain::State& s1,
                 const navigation::GraphDomain::State& s2) {
     if (!kVisualize) return;
     static const bool kDebug = false;
     if (kDebug) {
-      printf("%7.2f,%7.2f -> %7.2f,%7.2f\n",
-            s1.loc.x(),
-            s1.loc.y(),
-            s2.loc.x(),
-            s2.loc.y());
+      printf("%7.2f,%7.2f -> %7.2f,%7.2f\n", s1.loc.x(), s1.loc.y(), s2.loc.x(),
+             s2.loc.y());
     }
     // visualization::DrawLine(s1.loc, s2.loc, 0xC0C0C0, global_viz_msg_);
     // viz_pub_.publish(global_viz_msg_);
@@ -135,22 +125,22 @@ struct GraphVisualizer {
 
 namespace navigation {
 
-Navigation::Navigation() :
-    robot_loc_(0, 0),
-    robot_angle_(0),
-    robot_vel_(0, 0),
-    robot_omega_(0),
-    nav_state_(NavigationState::kStopped),
-    nav_goal_loc_(0, 0),
-    nav_goal_angle_(0),
-    odom_initialized_(false),
-    loc_initialized_(false),
-    t_point_cloud_(0),
-    t_odometry_(0),
-    enabled_(false),
-    initialized_(false),
-    sampler_(nullptr),
-    evaluator_(nullptr) {
+Navigation::Navigation()
+    : robot_loc_(0, 0),
+      robot_angle_(0),
+      robot_vel_(0, 0),
+      robot_omega_(0),
+      nav_state_(NavigationState::kStopped),
+      nav_goal_loc_(0, 0),
+      nav_goal_angle_(0),
+      odom_initialized_(false),
+      loc_initialized_(false),
+      t_point_cloud_(0),
+      t_odometry_(0),
+      enabled_(false),
+      initialized_(false),
+      sampler_(nullptr),
+      evaluator_(nullptr) {
   sampler_ = std::unique_ptr<PathRolloutSamplerBase>(new AckermannSampler());
 }
 
@@ -167,11 +157,11 @@ void Navigation::Initialize(const NavigationParameters& params,
     cost_map_evaluator->LoadModel();
     evaluator_ = cost_map_evaluator;
   } else if (params_.evaluator_type == "terrain") {
-    auto terrain_evaluator = std::make_shared<TerrainEvaluator>();
+    auto terrain_evaluator = std::make_shared<TerrainEvaluator>(params_);
     terrain_evaluator->LoadModel();
     evaluator_ = terrain_evaluator;
   } else if (params_.evaluator_type == "terrain2") {
-    auto terrain_evaluator = std::make_shared<CustomTerrainEvaluator>();
+    auto terrain_evaluator = std::make_shared<CustomTerrainEvaluator>(params_);
     terrain_evaluator->LoadModel();
     evaluator_ = terrain_evaluator;
   } else if (params_.evaluator_type == "linear") {
@@ -182,13 +172,9 @@ void Navigation::Initialize(const NavigationParameters& params,
   }
 }
 
-bool Navigation::Enabled() const {
-  return enabled_;
-}
+bool Navigation::Enabled() const { return enabled_; }
 
-void Navigation::Enable(bool enable) {
-  enabled_ = enable;
-}
+void Navigation::Enable(bool enable) { enabled_ = enable; }
 
 void Navigation::SetNavGoal(const Vector2f& loc, float angle) {
   nav_state_ = NavigationState::kGoto;
@@ -197,15 +183,12 @@ void Navigation::SetNavGoal(const Vector2f& loc, float angle) {
   plan_path_.clear();
 }
 
-
 void Navigation::SetOverride(const Vector2f& loc, float angle) {
   nav_state_ = NavigationState::kOverride;
   override_target_ = loc;
 }
 
-void Navigation::Resume() {
-  nav_state_ = NavigationState::kGoto;
-}
+void Navigation::Resume() { nav_state_ = NavigationState::kGoto; }
 
 void Navigation::UpdateMap(const string& map_path) {
   planning_domain_.Load(map_path);
@@ -229,12 +212,10 @@ void Navigation::PruneLatencyQueue() {
     }
     if (t_cmd < update_time - params_.dt) {
       if (kDebug) {
-        printf("Erase %d %f %f\n",
-            int(i),
-            t_cmd - update_time,
-            command_history_[i].linear.x());
+        printf("Erase %d %f %f\n", int(i), t_cmd - update_time,
+               command_history_[i].linear.x());
       }
-      command_history_.erase( command_history_.begin() + i);
+      command_history_.erase(command_history_.begin() + i);
       --i;
     }
   }
@@ -254,9 +235,8 @@ void Navigation::UpdateCommandHistory(Twist twist) {
   twist.time += params_.system_latency;
   command_history_.push_back(twist);
   if (false) {
-    printf("Push %f %f\n",
-          twist.linear.x(),
-          command_history_.back().linear.x());
+    printf("Push %f %f\n", twist.linear.x(),
+           command_history_.back().linear.x());
   }
 }
 
@@ -276,8 +256,8 @@ void Navigation::ForwardPredict(double t) {
     }
     printf("Predict: %f %f\n", t - t_odometry_, t - t_point_cloud_);
   }
-  odom_loc_ = Vector2f(latest_odom_msg_.position.x(),
-                       latest_odom_msg_.position.y());
+  odom_loc_ =
+      Vector2f(latest_odom_msg_.position.x(), latest_odom_msg_.position.y());
   odom_angle_ = 2.0f * atan2f(latest_odom_msg_.orientation.z(),
                               latest_odom_msg_.orientation.w());
   using Eigen::Affine2f;
@@ -288,21 +268,19 @@ void Navigation::ForwardPredict(double t) {
     const double cmd_time = c.time;
     if (cmd_time > t) continue;
     if (cmd_time >= t_odometry_ - params_.dt) {
-      const float dt = (t_odometry_ > cmd_time) ?
-          min<double>(t_odometry_ - cmd_time, params_.dt) :
-          min<double>(t - cmd_time, params_.dt);
-      odom_loc_ += dt * (Rotation2Df(odom_angle_) * Vector2f(
-          c.linear.x(), c.linear.y()));
+      const float dt = (t_odometry_ > cmd_time)
+                           ? min<double>(t_odometry_ - cmd_time, params_.dt)
+                           : min<double>(t - cmd_time, params_.dt);
+      odom_loc_ += dt * (Rotation2Df(odom_angle_) *
+                         Vector2f(c.linear.x(), c.linear.y()));
       odom_angle_ = AngleMod(odom_angle_ + dt * c.angular.z());
     }
-    if (t_point_cloud_ >= cmd_time  - params_.dt) {
-      const float dt = (t_point_cloud_ > cmd_time) ?
-          min<double>(t_point_cloud_ - cmd_time, params_.dt) :
-          min<double>(t - cmd_time, params_.dt);
-      lidar_tf =
-          Translation2f(-dt * Vector2f(c.linear.x(), c.linear.y())) *
-          Rotation2Df(-c.angular.z() * dt) *
-          lidar_tf;
+    if (t_point_cloud_ >= cmd_time - params_.dt) {
+      const float dt = (t_point_cloud_ > cmd_time)
+                           ? min<double>(t_point_cloud_ - cmd_time, params_.dt)
+                           : min<double>(t - cmd_time, params_.dt);
+      lidar_tf = Translation2f(-dt * Vector2f(c.linear.x(), c.linear.y())) *
+                 Rotation2Df(-c.angular.z() * dt) * lidar_tf;
     }
   }
   fp_point_cloud_.resize(point_cloud_.size());
@@ -316,15 +294,11 @@ void Navigation::TrapezoidTest(Vector2f& cmd_vel, float& cmd_angle_vel) {
   const float x = (odom_loc_ - starting_loc_).norm();
   const float speed = robot_vel_.norm();
   const float velocity_cmd = Run1DTimeOptimalControl(
-      params_.linear_limits,
-      x,
-      speed,
-      FLAGS_test_dist,
-      0,
-      params_.dt);
+      params_.linear_limits, x, speed, FLAGS_test_dist, 0, params_.dt);
   cmd_vel = {velocity_cmd, 0};
   cmd_angle_vel = 0;
-  printf("x: %.3f d:%.3f v: %.3f cmd:%.3f\n", x, FLAGS_test_dist, speed, velocity_cmd);
+  printf("x: %.3f d:%.3f v: %.3f cmd:%.3f\n", x, FLAGS_test_dist, speed,
+         velocity_cmd);
 }
 
 void Navigation::LatencyTest(Vector2f& cmd_vel, float& cmd_angle_vel) {
@@ -338,8 +312,7 @@ void Navigation::LatencyTest(Vector2f& cmd_vel, float& cmd_angle_vel) {
   static double t_start_ = GetMonotonicTime();
   const double t = GetMonotonicTime() - t_start_;
   // float v_current = robot_vel_.x();
-  float v_cmd = kMaxSpeed *
-      sin(2.0 * M_PI * kFrequency * t);
+  float v_cmd = kMaxSpeed * sin(2.0 * M_PI * kFrequency * t);
   cmd_vel = {v_cmd, 0};
   cmd_angle_vel = 0.0;
 }
@@ -359,12 +332,7 @@ void Navigation::ObstacleTest(Vector2f& cmd_vel, float& cmd_angle_vel) {
       max<float>(0.0f, free_path_length - params_.obstacle_margin);
   printf("%f\n", free_path_length);
   const float velocity_cmd = Run1DTimeOptimalControl(
-      params_.linear_limits,
-      0,
-      speed,
-      dist_left,
-      0,
-      params_.dt);
+      params_.linear_limits, 0, speed, dist_left, 0, params_.dt);
   cmd_vel = {velocity_cmd, 0};
   cmd_angle_vel = 0;
 }
@@ -395,13 +363,11 @@ Vector2f GetClosestApproach(const PathOption& o, const Vector2f& target) {
                      turn_radius * (1.0f - cos(end_angle)));
 
   Vector2f closest_point = start;
-  if (middle_angle < end_angle &&
-      (closest_point - target).squaredNorm() >
-      (middle - target).squaredNorm()) {
+  if (middle_angle < end_angle && (closest_point - target).squaredNorm() >
+                                      (middle - target).squaredNorm()) {
     closest_point = middle;
   }
-  if ((closest_point - target).squaredNorm() >
-      (end - target).squaredNorm()) {
+  if ((closest_point - target).squaredNorm() > (end - target).squaredNorm()) {
     closest_point = end;
   }
   return closest_point;
@@ -412,8 +378,7 @@ float GetClosestDistance(const PathOption& o, const Vector2f& target) {
   return (target - closest_point).norm();
 }
 
-void Navigation::ObservePointCloud(const vector<Vector2f>& cloud,
-                                   double time) {
+void Navigation::ObservePointCloud(const vector<Vector2f>& cloud, double time) {
   point_cloud_ = cloud;
   t_point_cloud_ = time;
   PruneLatencyQueue();
@@ -428,7 +393,7 @@ vector<int> Navigation::GlobalPlan(const Vector2f& initial,
                                    const Vector2f& end) {
   auto plan = Plan(initial, end);
   std::vector<int> path;
-  for (auto& node : plan ) {
+  for (auto& node : plan) {
     path.push_back(node.id);
   }
   return path;
@@ -468,14 +433,12 @@ void Navigation::PlannerTest() {
 }
 
 DEFINE_double(max_plan_deviation, 0.5,
-   "Maximum premissible deviation from the plan");
+              "Maximum premissible deviation from the plan");
 bool Navigation::PlanStillValid() {
   if (plan_path_.size() < 2) return false;
   for (size_t i = 0; i + 1 < plan_path_.size(); ++i) {
-    const float dist_from_segment =
-        geometry::DistanceFromLineSegment(robot_loc_,
-                                          plan_path_[i].loc,
-                                          plan_path_[i + 1].loc);
+    const float dist_from_segment = geometry::DistanceFromLineSegment(
+        robot_loc_, plan_path_[i].loc, plan_path_[i + 1].loc);
     if (dist_from_segment < FLAGS_max_plan_deviation) {
       return true;
     }
@@ -499,8 +462,8 @@ bool Navigation::GetCarrot(Vector2f& carrot) {
   for (size_t i = 0; i + 1 < plan_path_.size(); ++i) {
     const Vector2f v0 = plan_path_[i].loc;
     const Vector2f v1 = plan_path_[i + 1].loc;
-    const float dist_to_segment = geometry::DistanceFromLineSegment(
-        robot_loc_, v0, v1);
+    const float dist_to_segment =
+        geometry::DistanceFromLineSegment(robot_loc_, v0, v1);
     if (dist_to_segment < closest_dist) {
       closest_dist = dist_to_segment;
       i0 = i;
@@ -536,18 +499,22 @@ bool Navigation::GetCarrot(Vector2f& carrot) {
   const Vector2f v0 = plan_path_[i0].loc;
   const Vector2f v1 = plan_path_[i1].loc;
   Vector2f r0, r1;
-  #define V2COMP(v) v.x(), v.y()
+#define V2COMP(v) v.x(), v.y()
   // printf("%f,%f %f,%f %f,%f %f\n",
   //     V2COMP(robot_loc_), V2COMP(v0), V2COMP(v1), (v0 - v1).norm());
   const int num_intersections = geometry::CircleLineIntersection<float>(
       robot_loc_, params_.carrot_dist, v0, v1, &r0, &r1);
   if (num_intersections == 0) {
-    fprintf(stderr, "Error obtaining intersections:\n v0: (%f %f), v1: (%f %f), robot_loc_: (%f %f) sq_carrot_dist: (%f) closest_dist: (%f)\n",
-      v0.x(), v0.y(), v1.x(), v1.y(), robot_loc_.x(), robot_loc_.y(), kSqCarrotDist, closest_dist);
+    fprintf(stderr,
+            "Error obtaining intersections:\n v0: (%f %f), v1: (%f %f), "
+            "robot_loc_: (%f %f) sq_carrot_dist: (%f) closest_dist: (%f)\n",
+            v0.x(), v0.y(), v1.x(), v1.y(), robot_loc_.x(), robot_loc_.y(),
+            kSqCarrotDist, closest_dist);
     return false;
   }
 
-  if (num_intersections == 1 || (r0 - v1).squaredNorm() < (r1 - v1).squaredNorm()) {
+  if (num_intersections == 1 ||
+      (r0 - v1).squaredNorm() < (r1 - v1).squaredNorm()) {
     carrot = r0;
   } else {
     carrot = r1;
@@ -558,7 +525,8 @@ bool Navigation::GetCarrot(Vector2f& carrot) {
 void Navigation::GetStraightFreePathLength(float* free_path_length,
                                            float* clearance) {
   // How much the robot's body extends in front of its base link frame.
-  const float l = 0.5 * params_.robot_length - params_.base_link_offset + params_.obstacle_margin;
+  const float l = 0.5 * params_.robot_length - params_.base_link_offset +
+                  params_.obstacle_margin;
   // The robot's half-width.
   const float w = 0.5 * params_.robot_width + params_.obstacle_margin;
   for (const Vector2f& p : fp_point_cloud_) {
@@ -598,8 +566,10 @@ void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) {
     local_target = override_target_;
   }
 
-  sampler_->Update(robot_vel_, robot_omega_, local_target, fp_point_cloud_, latest_image_);
-  evaluator_->Update(robot_loc_, robot_angle_, robot_vel_, robot_omega_, local_target, fp_point_cloud_, latest_image_);
+  sampler_->Update(robot_vel_, robot_omega_, local_target, fp_point_cloud_,
+                   latest_image_);
+  evaluator_->Update(robot_loc_, robot_angle_, robot_vel_, robot_omega_,
+                     local_target, fp_point_cloud_, latest_image_);
   auto paths = sampler_->GetSamples(params_.num_options);
   if (debug) {
     printf("%lu options\n", paths.size());
@@ -607,8 +577,8 @@ void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) {
     for (auto p : paths) {
       ConstantCurvatureArc arc =
           *reinterpret_cast<ConstantCurvatureArc*>(p.get());
-      printf("%3d: %7.5f %7.3f %7.3f\n",
-          i++, arc.curvature, arc.length, arc.curvature);
+      printf("%3d: %7.5f %7.3f %7.3f\n", i++, arc.curvature, arc.length,
+             arc.curvature);
     }
   }
   if (paths.size() == 0) {
@@ -628,16 +598,12 @@ void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) {
   vel_cmd = {0, 0};
 
   float max_map_speed = params_.linear_limits.max_speed;
-  planning_domain_.GetClearanceAndSpeedFromLoc(
-      robot_loc_, nullptr, &max_map_speed);
+  planning_domain_.GetClearanceAndSpeedFromLoc(robot_loc_, nullptr,
+                                               &max_map_speed);
   auto linear_limits = params_.linear_limits;
   linear_limits.max_speed = min(max_map_speed, params_.linear_limits.max_speed);
-  best_path->GetControls(linear_limits,
-                         params_.angular_limits,
-                         params_.dt, robot_vel_,
-                         robot_omega_,
-                         vel_cmd,
-                         ang_vel_cmd);
+  best_path->GetControls(linear_limits, params_.angular_limits, params_.dt,
+                         robot_vel_, robot_omega_, vel_cmd, ang_vel_cmd);
   last_options_ = paths;
   best_option_ = best_path;
 }
@@ -657,7 +623,7 @@ void Navigation::Halt(Vector2f& cmd_vel, float& angular_vel_cmd) {
     }
   }
   cmd_vel = {velocity_cmd, 0};
-  //TODO: motion profiling for omega
+  // TODO: motion profiling for omega
   angular_vel_cmd = 0;
 }
 
@@ -678,9 +644,10 @@ void Navigation::TurnInPlace(Vector2f& cmd_vel, float& cmd_angle_vel) {
   } else if (nav_state_ == NavigationState::kTurnInPlace) {
     dTheta = AngleDiff(nav_goal_angle_, robot_angle_);
   }
-  if (kDebug) printf("dTheta: %f robot_angle: %f\n", RadToDeg(dTheta), RadToDeg(robot_angle_));
+  if (kDebug)
+    printf("dTheta: %f robot_angle: %f\n", RadToDeg(dTheta),
+           RadToDeg(robot_angle_));
 
-  
   const float s = Sign(dTheta);
   if (robot_omega_ * dTheta < 0.0f) {
     if (kDebug) printf("Wrong way\n");
@@ -692,20 +659,14 @@ void Navigation::TurnInPlace(Vector2f& cmd_vel, float& cmd_angle_vel) {
       cmd_angle_vel = robot_omega_ - Sign(robot_omega_) * dv;
     }
   } else {
-    cmd_angle_vel = s * Run1DTimeOptimalControl(
-        params_.angular_limits,
-        0,
-        s * robot_omega_,
-        s * dTheta,
-        0,
-        params_.dt);
+    cmd_angle_vel =
+        s * Run1DTimeOptimalControl(params_.angular_limits, 0, s * robot_omega_,
+                                    s * dTheta, 0, params_.dt);
   }
   cmd_vel = {0, 0};
 }
 
-void Navigation::Pause() {
-  nav_state_ = NavigationState::kPaused;
-}
+void Navigation::Pause() { nav_state_ = NavigationState::kPaused; }
 
 void Navigation::SetMaxVel(const float vel) {
   params_.linear_limits.max_speed = vel;
@@ -737,8 +698,7 @@ void Navigation::SetObstacleMargin(const float margin) {
 }
 
 void Navigation::SetClearanceWeight(const float weight) {
-  LinearEvaluator* evaluator =
-      dynamic_cast<LinearEvaluator*>(evaluator_.get());
+  LinearEvaluator* evaluator = dynamic_cast<LinearEvaluator*>(evaluator_.get());
   evaluator->SetClearanceWeight(weight);
   return;
 }
@@ -748,21 +708,13 @@ void Navigation::SetCarrotDist(const float carrot_dist) {
   return;
 }
 
-Eigen::Vector2f Navigation::GetTarget() {
-  return local_target_;
-}
+Eigen::Vector2f Navigation::GetTarget() { return local_target_; }
 
-Eigen::Vector2f Navigation::GetOverrideTarget() {
-  return override_target_;
-}
+Eigen::Vector2f Navigation::GetOverrideTarget() { return override_target_; }
 
-Eigen::Vector2f Navigation::GetVelocity() {
-  return robot_vel_;
-}
+Eigen::Vector2f Navigation::GetVelocity() { return robot_vel_; }
 
-float Navigation::GetAngularVelocity() {
-  return robot_omega_;
-}
+float Navigation::GetAngularVelocity() { return robot_omega_; }
 
 string Navigation::GetNavStatus() {
   switch (nav_state_) {
@@ -787,25 +739,15 @@ string Navigation::GetNavStatus() {
   }
 }
 
-vector<Vector2f> Navigation::GetPredictedCloud() {
-  return fp_point_cloud_;
-}
+vector<Vector2f> Navigation::GetPredictedCloud() { return fp_point_cloud_; }
 
-float Navigation::GetCarrotDist() {
-  return params_.carrot_dist;
-}
+float Navigation::GetCarrotDist() { return params_.carrot_dist; }
 
-float Navigation::GetObstacleMargin() {
-  return params_.obstacle_margin;
-}
+float Navigation::GetObstacleMargin() { return params_.obstacle_margin; }
 
-float Navigation::GetRobotWidth() {
-  return params_.robot_width;
-}
+float Navigation::GetRobotWidth() { return params_.robot_width; }
 
-float Navigation::GetRobotLength() {
-  return params_.robot_length;
-}
+float Navigation::GetRobotLength() { return params_.robot_length; }
 
 vector<std::shared_ptr<PathRolloutBase>> Navigation::GetLastPathOptions() {
   return last_options_;
@@ -813,9 +755,12 @@ vector<std::shared_ptr<PathRolloutBase>> Navigation::GetLastPathOptions() {
 
 const cv::Mat& Navigation::GetVisualizationImage() {
   if (params_.evaluator_type == "cost_map") {
-    return std::dynamic_pointer_cast<DeepCostMapEvaluator>(evaluator_)->latest_vis_image_;
-  } else if (params_.evaluator_type == "terrain" || params_.evaluator_type == "terrain2") {
-    return std::dynamic_pointer_cast<TerrainEvaluator>(evaluator_)->latest_vis_image_;
+    return std::dynamic_pointer_cast<DeepCostMapEvaluator>(evaluator_)
+        ->latest_vis_image_;
+  } else if (params_.evaluator_type == "terrain" ||
+             params_.evaluator_type == "terrain2") {
+    return std::dynamic_pointer_cast<TerrainEvaluator>(evaluator_)
+        ->latest_vis_image_;
   } else {
     std::cerr << "No visualization image for linear evaluator" << std::endl;
     exit(1);
@@ -825,8 +770,9 @@ const cv::Mat& Navigation::GetVisualizationImage() {
 const cv::Mat& Navigation::GetCostMapImage() {
   // TODO(eyang): this is a hack to get a two visualization images with and
   // without trajectory overlays.
-  
-  return std::dynamic_pointer_cast<TerrainEvaluator>(evaluator_)->latest_cost_image_;
+
+  return std::dynamic_pointer_cast<TerrainEvaluator>(evaluator_)
+      ->latest_cost_image_;
 }
 
 std::shared_ptr<PathRolloutBase> Navigation::GetOption() {
@@ -837,12 +783,9 @@ const GraphDomain& Navigation::GetPlanningDomain() const {
   return planning_domain_;
 }
 
-vector<GraphDomain::State> Navigation::GetPlanPath() {
-  return plan_path_;
-}
+vector<GraphDomain::State> Navigation::GetPlanPath() { return plan_path_; }
 
-bool Navigation::Run(const double& time,
-                     Vector2f& cmd_vel,
+bool Navigation::Run(const double& time, Vector2f& cmd_vel,
                      float& cmd_angle_vel) {
   const bool kDebug = FLAGS_v > 0;
   if (!initialized_) {
@@ -902,13 +845,12 @@ bool Navigation::Run(const double& time,
         robot_vel_.squaredNorm() < Sq(params_.target_dist_tolerance)) {
       nav_state_ = NavigationState::kTurnInPlace;
     } else if (nav_state_ == NavigationState::kTurnInPlace &&
-          AngleDist(robot_angle_, nav_goal_angle_) < 
-          params_.target_angle_tolerance) {
+               AngleDist(robot_angle_, nav_goal_angle_) <
+                   params_.target_angle_tolerance) {
       nav_state_ = NavigationState::kStopped;
     }
   } while (prev_state != nav_state_);
 
-  
   switch (nav_state_) {
     case NavigationState::kStopped: {
       if (kDebug) printf("\nNav complete\n");
@@ -926,8 +868,8 @@ bool Navigation::Run(const double& time,
       if (kDebug) printf("\nNav override\n");
     } break;
     default: {
-      fprintf(stderr, "ERROR: Unknown nav state %d\n", 
-          static_cast<int>(nav_state_));
+      fprintf(stderr, "ERROR: Unknown nav state %d\n",
+              static_cast<int>(nav_state_));
     }
   }
 
@@ -936,7 +878,7 @@ bool Navigation::Run(const double& time,
     Halt(cmd_vel, cmd_angle_vel);
     return true;
   } else if (nav_state_ == NavigationState::kGoto ||
-      nav_state_ == NavigationState::kOverride) {
+             nav_state_ == NavigationState::kOverride) {
     Vector2f local_target(0, 0);
     if (nav_state_ == NavigationState::kGoto) {
       // Local Navigation
diff --git a/src/navigation/terrain_evaluator.cc b/src/navigation/terrain_evaluator.cc
index b779022..ad1b9fb 100644
--- a/src/navigation/terrain_evaluator.cc
+++ b/src/navigation/terrain_evaluator.cc
@@ -27,9 +27,11 @@ CONFIG_FLOAT(clearance_weight, "TerrainEvaluator.clearance_weight");
 CONFIG_FLOAT(fpl_weight, "TerrainEvaluator.fpl_weight");
 CONFIG_FLOAT(terrain_weight, "TerrainEvaluator.terrain_weight");
 
-TerrainEvaluator::TerrainEvaluator()
+TerrainEvaluator::TerrainEvaluator(
+    const navigation::NavigationParameters& params)
     : cost_model_path_(CONFIG_model_path),
-      torch_device_(torch::cuda::is_available() ? torch::kCUDA : torch::kCPU) {}
+      torch_device_(torch::cuda::is_available() ? torch::kCUDA : torch::kCPU),
+      params_(params) {}
 
 bool TerrainEvaluator::LoadModel() {
   // Following the pytorch tutorial:
@@ -47,10 +49,9 @@ bool TerrainEvaluator::LoadModel() {
     cost_model_ = torch::jit::load(cost_model_path_, torch_device_);
     cost_model_.eval();
 
-    // print done loading 
+    // print done loading
     std::cout << "Done loading model" << std::endl;
 
-
     return true;
   } catch (const c10::Error& e) {
     LOG(ERROR) << "Unable to load model: \n" << e.msg();
@@ -62,8 +63,12 @@ std::shared_ptr<PathRolloutBase> TerrainEvaluator::FindBest(
     const std::vector<std::shared_ptr<PathRolloutBase>>& paths) {
   // This reassignment is simply to reduce ambiguity.
   const cv::Mat3b& latest_bev_image = image;
-  // print the shape
-  std::cout << "latest bev image " << latest_bev_image.rows << " " << latest_bev_image.cols << std::endl;
+
+  if (FLAGS_v > 3) {
+    // print the shape
+    std::cout << "latest bev image " << latest_bev_image.rows << " "
+              << latest_bev_image.cols << std::endl;
+  }
 
   if (latest_bev_image.rows == 0) {
     // cannot plan if the image is not initialized
@@ -83,16 +88,16 @@ std::shared_ptr<PathRolloutBase> TerrainEvaluator::FindBest(
   latest_cost_image_ = GetRGBCostImage(cost_image);
   latest_cost_image_.copyTo(latest_vis_image_);
 
-  // TODO(eyang): skipped a bunch of code dealing with other factors: distance to goal,
-  // clearance, progress, etc.
+  // TODO(eyang): skipped a bunch of code dealing with other factors: distance
+  // to goal, clearance, progress, etc.
 
   // Don't consider paths with endpoints that are blocked from the goal.
   std::vector<float> endpoint_clearance_to_goal(paths.size(), 0.0f);
   std::vector<float> endpoint_dist_to_goal(paths.size(), local_target.norm());
   for (size_t i = 0; i < paths.size(); ++i) {
     const Eigen::Vector2f path_endpoint = paths[i]->EndPoint().translation;
-    endpoint_clearance_to_goal[i] =
-        StraightLineClearance(geometry::Line2f(path_endpoint, local_target), point_cloud);
+    endpoint_clearance_to_goal[i] = StraightLineClearance(
+        geometry::Line2f(path_endpoint, local_target), point_cloud);
     // TODO(eyang): should this be a hyperparameter?
     if (endpoint_clearance_to_goal[i] > 0.05) {
       endpoint_dist_to_goal[i] = (path_endpoint - local_target).norm();
@@ -105,17 +110,19 @@ std::shared_ptr<PathRolloutBase> TerrainEvaluator::FindBest(
     float weight_sum = 0.0f;
 
     for (int j = 0; j <= CONFIG_rollout_density; j++) {
-      const pose_2d::Pose2Df state =
-          paths[i]->GetIntermediateState(static_cast<float>(j) / CONFIG_rollout_density);
+      const pose_2d::Pose2Df state = paths[i]->GetIntermediateState(
+          static_cast<float>(j) / CONFIG_rollout_density);
       const Eigen::Vector2i P_image_state =
           GetImageLocation(latest_bev_image, state.translation).cast<int>();
 
       // TODO: eventually in this case, ascribe the "out-of-view point" cost
       if (!ImageBoundCheck(cost_image, P_image_state)) {
-        LOG(ERROR) << "Cost image query point is outside the bounds of the image.";
+        LOG(ERROR)
+            << "Cost image query point is outside the bounds of the image.";
       }
 
-      const float center_cost = cost_image.at<float>(P_image_state.y(), P_image_state.x());
+      const float center_cost =
+          cost_image.at<float>(P_image_state.y(), P_image_state.x());
       if (center_cost > CONFIG_max_cost) {
         // indicates this point is not visible
         continue;
@@ -125,9 +132,8 @@ std::shared_ptr<PathRolloutBase> TerrainEvaluator::FindBest(
       float cost = 0;
       int num_valid_wheels = 0;
 
-      // TODO(eyang): use the robot width and length from config
-      const float robot_length = 0.6;
-      const float robot_width = 0.6;
+      const float robot_length = params_.robot_length;
+      const float robot_width = params_.robot_width;
       const Eigen::Rotation2Df state_rotation(state.angle);
 
       for (int i = 0; i < 4; i++) {
@@ -135,9 +141,12 @@ std::shared_ptr<PathRolloutBase> TerrainEvaluator::FindBest(
         const int x_side = 1 - (i & 0b10);
         const int y_side = 1 - 2 * (i & 0b1);
 
-        const Eigen::Vector2f P_robot_corner(x_side * robot_length / 2, y_side * robot_width / 2);
+        const Eigen::Vector2f P_robot_corner(x_side * robot_length / 2,
+                                             y_side * robot_width / 2);
         const Eigen::Vector2i P_image_corner =
-            GetImageLocation(latest_bev_image, state.translation + state_rotation * P_robot_corner)
+            GetImageLocation(
+                latest_bev_image,
+                state.translation + state_rotation * P_robot_corner)
                 .cast<int>();
 
         if (ImageBoundCheck(cost_image, P_image_corner)) {
@@ -152,7 +161,8 @@ std::shared_ptr<PathRolloutBase> TerrainEvaluator::FindBest(
         cost = CONFIG_max_cost;
       }
 
-      const float weight = std::pow(CONFIG_discount_factor, state.translation.norm());
+      const float weight =
+          std::pow(CONFIG_discount_factor, state.translation.norm());
       terrain_costs[i] += weight * cost;
       weight_sum += weight;
     }
@@ -170,9 +180,22 @@ std::shared_ptr<PathRolloutBase> TerrainEvaluator::FindBest(
   path_costs_ = std::vector<float>(paths.size(), best_path_cost);
   for (size_t i = 0; i < paths.size(); ++i) {
     const float path_progress = local_target.norm() - endpoint_dist_to_goal[i];
-    path_costs_[i] =
-        CONFIG_dist_to_goal_weight * path_progress + CONFIG_clearance_weight * paths[i]->FPL() +
-        CONFIG_clearance_weight * paths[i]->Clearance() + CONFIG_terrain_weight * terrain_costs[i];
+    path_costs_[i] = CONFIG_dist_to_goal_weight * path_progress +
+                     CONFIG_fpl_weight * paths[i]->FPL() +
+                     CONFIG_clearance_weight * paths[i]->Clearance() +
+                     CONFIG_terrain_weight * terrain_costs[i];
+    if (FLAGS_v > 3) {
+      printf("--- PRINTING PATH %ld COSTS ---\n", i);
+      printf("Progress: %f Progress Weight: %f\n", path_progress,
+             CONFIG_dist_to_goal_weight);
+      printf("Clearance: %f Clearance Weight: %f\n", paths[i]->Clearance(),
+             CONFIG_clearance_weight);
+      printf("FPL: %f FPL Weight: %f\n", paths[i]->FPL(), CONFIG_fpl_weight);
+      printf("Terrain: %f Terrain Weight: %f\n", terrain_costs[i],
+             CONFIG_terrain_weight);
+      printf("Total Cost: %f\n", path_costs_[i]);
+      printf("Best Path Cost: %f\n\n", best_path_cost);
+    }
 
     if (path_costs_[i] < best_path_cost) {
       best_path_cost = path_costs_[i];
@@ -183,8 +206,8 @@ std::shared_ptr<PathRolloutBase> TerrainEvaluator::FindBest(
   DrawPathCosts(paths, best_path);
   // save the image to a file
   // latest vis from rgb to bgr
-  // cv::cvtColor(latest_vis_image_, latest_vis_image_, cv::COLOR_RGB2BGR);
-  // cv::imwrite("latest_vis.png", latest_vis_image_);
+  cv::cvtColor(latest_vis_image_, latest_vis_image_, cv::COLOR_RGB2BGR);
+  cv::imwrite("latest_vis.png", latest_vis_image_);
   // cv::cvtColor(latest_vis_image_, latest_vis_image_, cv::COLOR_BGR2RGB);
   // cv::imwrite("latest_cost.png", latest_cost_image_);
   // static int functionCallCount = 0;
@@ -194,7 +217,6 @@ std::shared_ptr<PathRolloutBase> TerrainEvaluator::FindBest(
   //   exit(0);
   // }
 
-
   return best_path;
 }
 
@@ -211,7 +233,8 @@ cv::Mat1f TerrainEvaluator::GetScalarCostImage(const cv::Mat3b& bev_image) {
     for (int col = 0; col + CONFIG_patch_size_pixels <= bev_image.cols;
          col += CONFIG_patch_size_pixels) {
       // x, y, width, height
-      const cv::Rect bev_patch_roi(col, row, CONFIG_patch_size_pixels, CONFIG_patch_size_pixels);
+      const cv::Rect bev_patch_roi(col, row, CONFIG_patch_size_pixels,
+                                   CONFIG_patch_size_pixels);
 
       // Need to clone the patch into its own Mat because the underlying data
       // pointer of the submat is of the original image. The old code didn't
@@ -234,7 +257,8 @@ cv::Mat1f TerrainEvaluator::GetScalarCostImage(const cv::Mat3b& bev_image) {
       }
 
       torch::Tensor bev_patch_tensor = torch::from_blob(
-          bev_patch.data, {bev_patch.rows, bev_patch.cols, bev_patch.channels()}, torch::kByte);
+          bev_patch.data,
+          {bev_patch.rows, bev_patch.cols, bev_patch.channels()}, torch::kByte);
 
       // Clone the tensor to take ownership of the underlying cv::Mat3b patch
       // data that will go out of scope.
@@ -243,10 +267,11 @@ cv::Mat1f TerrainEvaluator::GetScalarCostImage(const cv::Mat3b& bev_image) {
       // This conversion might be necessary if the model's forward does not
       // convert the tensor to float. The model's expected behavior should be
       // standardized.
-      // bev_patch_tensor = bev_patch_tensor.to(torch::kFloat);
-
-      bev_patch_tensor = bev_patch_tensor.flip(2);  // BGR -> RGB
+      bev_patch_tensor = bev_patch_tensor.to(torch::kFloat);
 
+      // bev_patch_tensor = bev_patch_tensor.flip(2);  // BGR -> RGB
+      // normalize
+      bev_patch_tensor = bev_patch_tensor.to(torch::kFloat32) / 255.0;
       bev_patch_tensor = bev_patch_tensor.permute({2, 0, 1});
 
       bev_patch_tensors.push_back(bev_patch_tensor);
@@ -254,25 +279,29 @@ cv::Mat1f TerrainEvaluator::GetScalarCostImage(const cv::Mat3b& bev_image) {
     }
   }
 
-  torch::Tensor all_input_tensors = torch::stack(bev_patch_tensors).to(torch_device_);
+  torch::Tensor all_input_tensors =
+      torch::stack(bev_patch_tensors).to(torch_device_);
 
   torch::NoGradGuard no_grad;
   const size_t BATCH_SIZE = 32;
-  const int N_BATCHES = (bev_patch_tensors.size() + BATCH_SIZE - 1) / BATCH_SIZE;
+  const int N_BATCHES =
+      (bev_patch_tensors.size() + BATCH_SIZE - 1) / BATCH_SIZE;
   std::vector<torch::Tensor> batch_outputs;
 
   for (int batch = 0; batch < N_BATCHES; ++batch) {
     int batch_start_idx = batch * BATCH_SIZE;
-    int batch_end_idx = std::min(batch_start_idx + BATCH_SIZE, bev_patch_tensors.size());
+    int batch_end_idx =
+        std::min(batch_start_idx + BATCH_SIZE, bev_patch_tensors.size());
 
-    torch::Tensor batch_tensor =
-        all_input_tensors.index({torch::indexing::Slice(batch_start_idx, batch_end_idx)});
+    torch::Tensor batch_tensor = all_input_tensors.index(
+        {torch::indexing::Slice(batch_start_idx, batch_end_idx)});
 
     // Type conversion to a vector of IValues is necessary for Module::forward
     std::vector<torch::jit::IValue> model_inputs;
     model_inputs.push_back(batch_tensor);
 
-    batch_outputs.push_back(cost_model_.forward(model_inputs).toTensor().to(torch::kCPU));
+    batch_outputs.push_back(
+        cost_model_.forward(model_inputs).toTensor().to(torch::kCPU));
   }
 
   torch::Tensor model_output_tensor = torch::cat(batch_outputs, 0).squeeze();
@@ -290,21 +319,26 @@ cv::Mat1f TerrainEvaluator::GetScalarCostImage(const cv::Mat3b& bev_image) {
   return cost_image;
 }
 
-cv::Mat3b TerrainEvaluator::GetRGBCostImage(const cv::Mat1f& scalar_cost_image) {
-  cv::Mat3b rgb_cost_image = cv::Mat3b(scalar_cost_image.rows, scalar_cost_image.cols);
+cv::Mat3b TerrainEvaluator::GetRGBCostImage(
+    const cv::Mat1f& scalar_cost_image) {
+  cv::Mat3b rgb_cost_image =
+      cv::Mat3b(scalar_cost_image.rows, scalar_cost_image.cols);
 
-  cv::Mat1f intensity = (scalar_cost_image - CONFIG_min_cost) / (CONFIG_max_cost - CONFIG_min_cost);
+  cv::Mat1f intensity = (scalar_cost_image - CONFIG_min_cost) /
+                        (CONFIG_max_cost - CONFIG_min_cost);
   // Clamp the intensity values
   cv::min(intensity, 1.f, intensity);
   cv::max(intensity, 0.f, intensity);
   // Convert to byte range
   intensity *= 255;
 
-  // TODO(eyang): there might be a builtin function that can do this copy more effiently
+  // TODO(eyang): there might be a builtin function that can do this copy more
+  // effiently
   for (int row = 0; row < rgb_cost_image.rows; ++row) {
     for (int col = 0; col < rgb_cost_image.cols; ++col) {
       if (scalar_cost_image(row, col) <= CONFIG_max_cost) {
-        rgb_cost_image.at<cv::Vec3b>(row, col) = cv::Vec3b::all(intensity(row, col));
+        rgb_cost_image.at<cv::Vec3b>(row, col) =
+            cv::Vec3b::all(intensity(row, col));
       } else {
         rgb_cost_image.at<cv::Vec3b>(row, col) = cv::Vec3b(255, 0, 0);
       }
@@ -314,14 +348,14 @@ cv::Mat3b TerrainEvaluator::GetRGBCostImage(const cv::Mat1f& scalar_cost_image)
   return rgb_cost_image;
 }
 
-Eigen::Vector2f TerrainEvaluator::GetImageLocation(const cv::Mat3b& img,
-                                                   const Eigen::Vector2f& P_robot) {
+Eigen::Vector2f TerrainEvaluator::GetImageLocation(
+    const cv::Mat3b& img, const Eigen::Vector2f& P_robot) {
   // TODO(eyang): de-hardcode
   // TODO(eyang): For a single image, the robot's location is assumed to at the
   // center-bottom of the image. Eventually a fused BEV image may be used, where
   // the robot's location will be the center of the image.
-  // TODO(eyang): maybe some latched msg or configuration for the center would be appropriate?
-  // Location of the Robot's (0, 0) in the image
+  // TODO(eyang): maybe some latched msg or configuration for the center would
+  // be appropriate? Location of the Robot's (0, 0) in the image
   const Eigen::Vector2f P_image_robot(img.cols / 2, img.rows - 1);
 
   // Relative image coordinates of the query point.
@@ -333,29 +367,31 @@ Eigen::Vector2f TerrainEvaluator::GetImageLocation(const cv::Mat3b& img,
   return P_image;
 }
 
-cv::Rect TerrainEvaluator::GetPatchRectAtLocation(const cv::Mat3b& img,
-                                                  const Eigen::Vector2f& P_robot) {
+cv::Rect TerrainEvaluator::GetPatchRectAtLocation(
+    const cv::Mat3b& img, const Eigen::Vector2f& P_robot) {
   const Eigen::Vector2f P_image = GetImageLocation(img, P_robot);
 
   // Top-left coordinates of patch
-  int patch_tl_x =
-      static_cast<int>(P_image.x() / CONFIG_patch_size_pixels) * CONFIG_patch_size_pixels;
-  int patch_tl_y =
-      static_cast<int>(P_image.y() / CONFIG_patch_size_pixels) * CONFIG_patch_size_pixels;
+  int patch_tl_x = static_cast<int>(P_image.x() / CONFIG_patch_size_pixels) *
+                   CONFIG_patch_size_pixels;
+  int patch_tl_y = static_cast<int>(P_image.y() / CONFIG_patch_size_pixels) *
+                   CONFIG_patch_size_pixels;
 
-  return {patch_tl_x, patch_tl_y, CONFIG_patch_size_pixels, CONFIG_patch_size_pixels};
+  return {patch_tl_x, patch_tl_y, CONFIG_patch_size_pixels,
+          CONFIG_patch_size_pixels};
 }
 
-void TerrainEvaluator::DrawPathCosts(const std::vector<std::shared_ptr<PathRolloutBase>>& paths,
-                                     std::shared_ptr<PathRolloutBase> best_path) {
+void TerrainEvaluator::DrawPathCosts(
+    const std::vector<std::shared_ptr<PathRolloutBase>>& paths,
+    std::shared_ptr<PathRolloutBase> best_path) {
   // TODO(eyang): perhaps have some toggle between the cost map image and the
   // BEV image? might be useful to have both options. Maybe this toggle should
   // be in FindBest, because that's where the latest_vis_image_ is set.
 
   // Normalize all costs linearly to be within [0, 1]
   std::vector<float> normalized_path_costs(path_costs_);
-  const auto minmax_costs =
-      std::minmax_element(normalized_path_costs.begin(), normalized_path_costs.end());
+  const auto minmax_costs = std::minmax_element(normalized_path_costs.begin(),
+                                                normalized_path_costs.end());
   const float min_cost = *minmax_costs.first;
   const float max_cost = *minmax_costs.second;
   for (float& cost : normalized_path_costs) {
@@ -364,11 +400,13 @@ void TerrainEvaluator::DrawPathCosts(const std::vector<std::shared_ptr<PathRollo
 
   for (size_t i = 0; i < paths.size(); i++) {
     for (int j = 0; j < CONFIG_rollout_density; j++) {
-      const pose_2d::Pose2Df state =
-          paths[i]->GetIntermediateState(static_cast<float>(j) / CONFIG_rollout_density);
-      const Eigen::Vector2f P_image_state = GetImageLocation(latest_vis_image_, state.translation);
+      const pose_2d::Pose2Df state = paths[i]->GetIntermediateState(
+          static_cast<float>(j) / CONFIG_rollout_density);
+      const Eigen::Vector2f P_image_state =
+          GetImageLocation(latest_vis_image_, state.translation);
 
-      // Scale the color from green to yellow to red based on the normalized cost
+      // Scale the color from green to yellow to red based on the normalized
+      // cost
       cv::Scalar color;  // RGB
       if (normalized_path_costs[i] < 0.5) {
         // green set to 255, increasing red changes color from green to yellow
@@ -385,21 +423,25 @@ void TerrainEvaluator::DrawPathCosts(const std::vector<std::shared_ptr<PathRollo
         // a negative thickness value fills in the drawn circle
         thickness = -thickness;
       }
-      cv::circle(latest_vis_image_, cv::Point(P_image_state.x(), P_image_state.y()), 8, color,
+      cv::circle(latest_vis_image_,
+                 cv::Point(P_image_state.x(), P_image_state.y()), 8, color,
                  thickness, cv::LineTypes::LINE_AA);
     }
 
     // print normalized path costs[i]
-    // std::cout << "Normalized path costs " << normalized_path_costs[i] << std::endl;
+    // std::cout << "Normalized path costs " << normalized_path_costs[i] <<
+    // std::endl;
   }
 
-  const Eigen::Vector2f P_image_goal = GetImageLocation(latest_vis_image_, local_target);
-  cv::drawMarker(latest_vis_image_, cv::Point(P_image_goal.x(), P_image_goal.y()), {255, 255, 255},
+  const Eigen::Vector2f P_image_goal =
+      GetImageLocation(latest_vis_image_, local_target);
+  cv::drawMarker(latest_vis_image_,
+                 cv::Point(P_image_goal.x(), P_image_goal.y()), {255, 255, 255},
                  cv::MARKER_TILTED_CROSS, 32, 8, cv::LineTypes::LINE_AA);
-  
-  // print he best path and its cost
-  // std::cout << "Best path " << best_path << " " << normalized_path_costs[0] << std::endl;
 
+  // print he best path and its cost
+  // std::cout << "Best path " << best_path << " " << normalized_path_costs[0]
+  // << std::endl;
 }
 
 }  // namespace motion_primitives
diff --git a/src/navigation/terrain_evaluator.h b/src/navigation/terrain_evaluator.h
index a0c5a39..aa5ac9d 100644
--- a/src/navigation/terrain_evaluator.h
+++ b/src/navigation/terrain_evaluator.h
@@ -12,7 +12,7 @@ namespace motion_primitives {
 
 class TerrainEvaluator : public PathEvaluatorBase {
  public:
-  TerrainEvaluator();
+  TerrainEvaluator(const navigation::NavigationParameters& params);
 
   bool LoadModel();
 
@@ -34,11 +34,12 @@ class TerrainEvaluator : public PathEvaluatorBase {
 
  protected:
   /**
-   * Returns whether the specified [x, y] image coordinate is a valid pixel of the
-   * specified image.
+   * Returns whether the specified [x, y] image coordinate is a valid pixel of
+   * the specified image.
    */
   template <typename ScalarT>
-  bool ImageBoundCheck(const cv::Mat& image, const Eigen::Matrix<ScalarT, 2, 1>& P_image) {
+  bool ImageBoundCheck(const cv::Mat& image,
+                       const Eigen::Matrix<ScalarT, 2, 1>& P_image) {
     return 0 <= P_image.x() && P_image.x() < image.cols && 0 <= P_image.y() &&
            P_image.y() < image.rows;
   }
@@ -47,13 +48,15 @@ class TerrainEvaluator : public PathEvaluatorBase {
    * Return the approximate pixel location of a coordinate in the robot's local
    * reference frame.
    */
-  Eigen::Vector2f GetImageLocation(const cv::Mat3b& img, const Eigen::Vector2f& P_robot);
+  Eigen::Vector2f GetImageLocation(const cv::Mat3b& img,
+                                   const Eigen::Vector2f& P_robot);
 
   /**
    * Get the binned (multiple of patch size) patch bounding image coordinates
    * corresponding to a coordinate in the robot's local reference frame.
    */
-  cv::Rect GetPatchRectAtLocation(const cv::Mat3b& img, const Eigen::Vector2f& P_robot);
+  cv::Rect GetPatchRectAtLocation(const cv::Mat3b& img,
+                                  const Eigen::Vector2f& P_robot);
 
   /**
    * Annotates the latest_vis_image with intermediate points along each path
@@ -67,10 +70,12 @@ class TerrainEvaluator : public PathEvaluatorBase {
                      std::shared_ptr<PathRolloutBase> best_path);
 
  public:
-  // TODO: this is public to match DeepCostMapEvaluator: perhaps an accessor method
-  // is more appropriate.
-  cv::Mat3b latest_vis_image_ = cv::Mat3b::zeros(8, 8);  // has trajectory rollouts
-  cv::Mat3b latest_cost_image_ = cv::Mat3b::zeros(8, 8);  // does not have trajectory rollouts
+  // TODO: this is public to match DeepCostMapEvaluator: perhaps an accessor
+  // method is more appropriate.
+  cv::Mat3b latest_vis_image_ =
+      cv::Mat3b::zeros(8, 8);  // has trajectory rollouts
+  cv::Mat3b latest_cost_image_ =
+      cv::Mat3b::zeros(8, 8);  // does not have trajectory rollouts
 
   std::vector<float> path_costs_;
 
@@ -80,6 +85,7 @@ class TerrainEvaluator : public PathEvaluatorBase {
   std::string cost_model_path_;
   torch::jit::Module cost_model_;
   torch::Device torch_device_;
+  const navigation::NavigationParameters& params_;
 
   // Use CONFIG_* macros directly for automatic lua script reloading.
   /*
diff --git a/src/navigation/terrain_evaluator2.h b/src/navigation/terrain_evaluator2.h
index 717e6a9..d26514d 100644
--- a/src/navigation/terrain_evaluator2.h
+++ b/src/navigation/terrain_evaluator2.h
@@ -3,34 +3,35 @@
 namespace motion_primitives {
 CONFIG_STRING(context_path, "Context.context_path");
 
-
-
 class CustomTerrainEvaluator : public TerrainEvaluator {
  public:
- torch::Tensor context_tensor_;
-  CustomTerrainEvaluator() : TerrainEvaluator() {
+  torch::Tensor context_tensor_;
+  CustomTerrainEvaluator(const navigation::NavigationParameters& params)
+      : TerrainEvaluator(params) {
     // todo: store the context as private variable
     torch::jit::script::Module tensors = torch::jit::load(CONFIG_context_path);
     context_tensor_ = tensors.run_method("return_tensor").toTensor();
   }
 
-
-
-// latest bev image 749 1476
-// latest bev image channels 3
-// Image min: 0
-// Image max: 255
+  // latest bev image 749 1476
+  // latest bev image channels 3
+  // Image min: 0
+  // Image max: 255
 
   cv::Mat1f GetScalarCostImage(const cv::Mat3b& bev_image) override {
     // print here
     // std::cout << "Custom GetScalarCostImage" << std::endl;
     cv::Mat3b latest_bev_image = image.clone();
     // print the shape
-    std::cout << "latest bev image " << latest_bev_image.rows << " " << latest_bev_image.cols << std::endl;
+    if (FLAGS_v > 2) {
+      std::cout << "latest bev image " << latest_bev_image.rows << " "
+                << latest_bev_image.cols << std::endl;
+      // print the number of channels
+      std::cout << "latest bev image channels " << latest_bev_image.channels()
+                << std::endl;
+    }
     int rows = latest_bev_image.rows;
     int cols = latest_bev_image.cols;
-    // print the number of channels
-    std::cout << "latest bev image channels " << latest_bev_image.channels() << std::endl;
 
     // // for debugging
     // cv::Mat1f scalar_cost_map(rows, cols);
@@ -51,13 +52,14 @@ class CustomTerrainEvaluator : public TerrainEvaluator {
     // cv::imwrite("scalar_cost_map.png", scalar_cost_map*255);
     // return scalar_cost_map;
 
-
     // resize the image
     cv::resize(latest_bev_image, latest_bev_image, cv::Size(256, 128));
 
     // swap the channels bgr to rgb
-    cv::cvtColor(latest_bev_image, latest_bev_image, cv::COLOR_BGR2RGB);
-    auto img_tensor = torch::from_blob(latest_bev_image.data, {1, latest_bev_image.rows, latest_bev_image.cols, 3}, torch::kByte);
+    // cv::cvtColor(latest_bev_image, latest_bev_image, cv::COLOR_BGR2RGB);
+    auto img_tensor = torch::from_blob(
+        latest_bev_image.data,
+        {1, latest_bev_image.rows, latest_bev_image.cols, 3}, torch::kByte);
     img_tensor = img_tensor.permute({0, 3, 1, 2});
     // convert to float between 0 and 1
     img_tensor = img_tensor.to(torch::kFloat32);
@@ -69,7 +71,6 @@ class CustomTerrainEvaluator : public TerrainEvaluator {
     // std::cout << size << " ";
     // }
     // std::cout << std::endl;
-    
 
     // print the shape
     // std::cout << "Image shape: ";
@@ -78,10 +79,11 @@ class CustomTerrainEvaluator : public TerrainEvaluator {
     // }
 
     // print the min and max
-    std::cout << std::endl;
-    std::cout << "Image min: " << img_tensor.min().item<float>() << std::endl;
-    std::cout << "Image max: " << img_tensor.max().item<float>() << std::endl;
-
+    if (FLAGS_v > 2) {
+      std::cout << std::endl;
+      std::cout << "Image min: " << img_tensor.min().item<float>() << std::endl;
+      std::cout << "Image max: " << img_tensor.max().item<float>() << std::endl;
+    }
 
     std::vector<torch::jit::IValue> inputs;
     inputs.push_back(context_tensor_.to(torch_device_));
@@ -95,41 +97,43 @@ class CustomTerrainEvaluator : public TerrainEvaluator {
     // apply sigmoid to the output
     output = torch::sigmoid(output).to(torch::kCPU);
 
-    
-    // Print the shape of the output
-    std::cout << "Output shape: ";
-    for (auto& size : output.sizes()) {
-    std::cout << size << " ";
+    if (FLAGS_v > 2) {
+      // Print the shape of the output
+      std::cout << "Output shape: ";
+      for (auto& size : output.sizes()) {
+        std::cout << size << " ";
+      }
+      std::cout << std::endl;
+
+      // Print the min and max of the output
+      std::cout << "Output min: " << output.min().item<float>() << std::endl;
+      std::cout << "Output max: " << output.max().item<float>() << std::endl;
     }
-    std::cout << std::endl;
-
-    // Print the min and max of the output
-    std::cout << "Output min: " << output.min().item<float>() << std::endl;
-    std::cout << "Output max: " << output.max().item<float>() << std::endl;
 
     // Convert the output to a cv::Mat
     cv::Mat1f scalar_cost_map(output.size(2), output.size(3));
-    std::memcpy(scalar_cost_map.data, output.data_ptr(), output.numel() * sizeof(float));
+    std::memcpy(scalar_cost_map.data, output.data_ptr(),
+                output.numel() * sizeof(float));
 
     // resize the scalar_cost_map to the original size
     cv::resize(scalar_cost_map, scalar_cost_map, cv::Size(cols, rows));
     // print the new size
-    // std::cout << "Scalar cost map size: " << scalar_cost_map.rows << " " << scalar_cost_map.cols << std::endl;
+    // std::cout << "Scalar cost map size: " << scalar_cost_map.rows << " " <<
+    // scalar_cost_map.cols << std::endl;
 
     // todo: out-of-view cost
 
-
-
-    // // write the latest_bev_image to a file
-    // cv::imwrite("latest_bev_image.png", latest_bev_image);
-    // // write the scalar_cost_map to a file
-    // cv::imwrite("scalar_cost_map.png", scalar_cost_map*255);
+    if (FLAGS_v > 2) {
+      printf("Writing terrain2 costmap images\n");
+      // write the latest_bev_image to a file
+      cv::imwrite("latest_bev_image.png", latest_bev_image);
+      // write the scalar_cost_map to a file
+      cv::imwrite("scalar_cost_map.png", scalar_cost_map * 255);
+    }
 
     // return the output
     return scalar_cost_map;
-
-    
   }
 };
 
-}  // namespace motion_primitives
\ No newline at end of file
+}  // namespace motion_primitives