diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp
index b8036b635..515ee3cce 100644
--- a/gtsam/basis/Chebyshev2.cpp
+++ b/gtsam/basis/Chebyshev2.cpp
@@ -59,6 +59,15 @@ Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
   return weights / d;
 }
 
+Matrix Chebyshev2::WeightMatrix(size_t N, const Vector& X, double a, double b) {
+  // Chebyshev points go from 0 to N, hence N+1 points.
+  Matrix W(X.size(), N + 1);
+  for (int i = 0; i < X.size(); i++) {
+    W.row(i) = CalculateWeights(N, X(i), a, b);
+  }
+  return W;
+}
+
 Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
   // Allocate space for weights
   Weights weightDerivatives(N + 1);
diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h
index f7537eb64..1f7e50a03 100644
--- a/gtsam/basis/Chebyshev2.h
+++ b/gtsam/basis/Chebyshev2.h
@@ -102,6 +102,16 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
   static Weights CalculateWeights(size_t N, double x, double a = -1,
                                   double b = 1);
 
+  /**
+   * Calculate weights for all x in vector X.
+   * Returns M*N matrix where M is the size of the vector X,
+   * and N is the number of basis functions.
+   *
+   * Overriden for Chebyshev2.
+   */
+  static Matrix WeightMatrix(size_t N, const Vector& X, double a = -1,
+                             double b = 1);
+
   /**
    *  Evaluate derivative of barycentric weights.
    *  This is easy and efficient via the DifferentiationMatrix.
diff --git a/python/gtsam/tests/test_basis.py b/python/gtsam/tests/test_basis.py
index 5d3c5ace3..af423d10d 100644
--- a/python/gtsam/tests/test_basis.py
+++ b/python/gtsam/tests/test_basis.py
@@ -11,10 +11,9 @@ Author: Frank Dellaert & Gerry Chen (Python)
 import unittest
 
 import numpy as np
+from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
-from gtsam.utils.test_case import GtsamTestCase
-from gtsam.symbol_shorthand import B
 
 
 class TestBasis(GtsamTestCase):
@@ -26,6 +25,7 @@ class TestBasis(GtsamTestCase):
     Chebyshev bases, the line y=x is used to generate the data while for Fourier, 0.7*cos(x) is
     used.
     """
+
     def setUp(self):
         self.N = 2
         self.x = [0., 0.5, 0.75]