Make ConditionedCostFunction compatible with repeated CostFunction.

If the user uses the same conditioner twice, it does not lead
to a double free errors.

https://github.com/ceres-solver/ceres-solver/issues/422

Change-Id: I9041ddcbffa8dcb882a63bddb82b384897efc970

include/ceres/conditioned_cost_function.h

diff --git a/include/ceres/conditioned_cost_function.h b/include/ceres/conditioned_cost_function.h
index e5f5fc4..f92787e 100644
--- a/include/ceres/conditioned_cost_function.h
+++ b/include/ceres/conditioned_cost_function.h
@@ -77,6 +77,8 @@
   // per-residual conditioner. Takes ownership of all of the wrapped cost
   // functions, or not, depending on the ownership parameter. Conditioners
   // may be NULL, in which case the corresponding residual is not modified.
+  //
+  // The conditioners can repeat.
   ConditionedCostFunction(CostFunction* wrapped_cost_function,
                           const std::vector<CostFunction*>& conditioners,
                           Ownership ownership);

internal/ceres/conditioned_cost_function.cc

diff --git a/internal/ceres/conditioned_cost_function.cc b/internal/ceres/conditioned_cost_function.cc
index 08899e3..d933ad7 100644
--- a/internal/ceres/conditioned_cost_function.cc
+++ b/internal/ceres/conditioned_cost_function.cc
@@ -68,7 +68,7 @@
 
 ConditionedCostFunction::~ConditionedCostFunction() {
   if (ownership_ == TAKE_OWNERSHIP) {
-    STLDeleteElements(&conditioners_);
+    STLDeleteUniqueContainerPointers(conditioners_.begin(), conditioners_.end());
   } else {
     wrapped_cost_function_.release();
   }

internal/ceres/conditioned_cost_function_test.cc

diff --git a/internal/ceres/conditioned_cost_function_test.cc b/internal/ceres/conditioned_cost_function_test.cc
index 528e0ee..6297451 100644
--- a/internal/ceres/conditioned_cost_function_test.cc
+++ b/internal/ceres/conditioned_cost_function_test.cc
@@ -67,7 +67,7 @@
 };
 
 // Tests that ConditionedCostFunction does what it's supposed to.
-TEST(CostFunctionTest, ConditionedCostFunction) {
+TEST(ConditionedCostFunction, NormalOperation) {
   double v1[kTestCostFunctionSize], v2[kTestCostFunctionSize],
       jac[kTestCostFunctionSize * kTestCostFunctionSize],
       result[kTestCostFunctionSize];
@@ -92,17 +92,16 @@
     conditioners.push_back(new LinearCostFunction(i + 2, i * 7));
   }
 
-  ConditionedCostFunction conditioned_cost_function(difference_cost_function,
-                                                    conditioners,
-                                                    TAKE_OWNERSHIP);
+  ConditionedCostFunction conditioned_cost_function(
+      difference_cost_function, conditioners, TAKE_OWNERSHIP);
   EXPECT_EQ(difference_cost_function->num_residuals(),
             conditioned_cost_function.num_residuals());
   EXPECT_EQ(difference_cost_function->parameter_block_sizes(),
             conditioned_cost_function.parameter_block_sizes());
 
-  double *parameters[1];
+  double* parameters[1];
   parameters[0] = v1;
-  double *jacs[1];
+  double* jacs[1];
   jacs[0] = jac;
 
   conditioned_cost_function.Evaluate(parameters, result, jacs);
@@ -122,5 +121,21 @@
   }
 }
 
+TEST(ConditionedCostFunction, SharedConditionersDoNotTriggerDoubleFree) {
+  // Make a cost function that computes x - v2
+  double v2[kTestCostFunctionSize];
+  VectorRef v2_vector(v2, kTestCostFunctionSize, 1);
+  Matrix identity = Matrix::Identity(kTestCostFunctionSize, kTestCostFunctionSize);
+  NormalPrior* difference_cost_function = new NormalPrior(identity, v2_vector);
+  CostFunction* conditioner = new LinearCostFunction(2, 7);
+  std::vector<CostFunction*> conditioners;
+  for (int i = 0; i < kTestCostFunctionSize; i++) {
+    conditioners.push_back(conditioner);
+  }
+
+  ConditionedCostFunction conditioned_cost_function(
+      difference_cost_function, conditioners, TAKE_OWNERSHIP);
+}
+
 }  // namespace internal
 }  // namespace ceres