Simplify GenerateCodeForFunctor

- Don't trace residual -> Use input cost functor instead
- Remove unnecessary copy from tmp jacobian array
- Fix indent of generated Evaluate() function

Change-Id: I23e09987d8ec30f7202a93eb253648505dcaaa4e

include/ceres/codegen/generate_code_for_functor.h

diff --git a/include/ceres/codegen/generate_code_for_functor.h b/include/ceres/codegen/generate_code_for_functor.h
index 31ccdb2..d7ed09e 100644
--- a/include/ceres/codegen/generate_code_for_functor.h
+++ b/include/ceres/codegen/generate_code_for_functor.h
@@ -123,12 +123,6 @@
     internal::MakeOutput(J.a, "residuals[" + std::to_string(i) + "]");
   }
 
-  // Make a copy of the current graph so we can generated a function for the
-  // residuals without jacobians.
-  auto residual_graph = *internal::GetCurrentExpressionGraph();
-
-  // Same principle as above for the residuals.
-  //
   // Example code generated by these expressions:
   //    jacobians[0][0] = v_351;
   //    jacobians[0][1] = v_352;
@@ -162,20 +156,6 @@
   output.emplace_back("");
 
   {
-    // Generate C++ code for the EvaluateResidual function and append it to the
-    // output.
-    internal::CodeGenerator::Options generator_options;
-    generator_options.function_name =
-        "void EvaluateResidual(double const* const* parameters, double* "
-        "residuals) const";
-    internal::CodeGenerator gen(residual_graph, generator_options);
-    std::vector<std::string> code = gen.Generate();
-    output.insert(output.end(), code.begin(), code.end());
-  }
-
-  output.emplace_back("");
-
-  {
     // Generate C++ code for the EvaluateResidualAndJacobian function and append
     // it to the output.
     internal::CodeGenerator::Options generator_options;
@@ -198,63 +178,40 @@
   output.emplace_back("              double* residuals,");
   output.emplace_back("              double** jacobians) const {");
 
-  output.emplace_back("   if (!jacobians) {");
-  output.emplace_back("     EvaluateResidual(parameters, residuals);");
-  output.emplace_back("     return true;");
-  output.emplace_back("   }");
+  output.emplace_back("  if (!jacobians) {");
+  output.emplace_back("    // Use the input cost functor");
+  output.emplace_back("    return (*this)(");
+  for (int i = 0; i < kNumParameterBlocks; ++i) {
+    output.emplace_back("      parameters[" + std::to_string(i) + "],");
+  }
+  output.emplace_back("      residuals");
+  output.emplace_back("    );");
+  output.emplace_back("  }");
 
-  // Create a tmp array of all jacobians and use it for evaluation.
-  // The generated code for a <2,3,1,2> cost functor is:
+  // Create a tmp array of all jacobians and use it for evaluation if the input
+  // jacobian is null. The generated code for a <2,3,1,2> cost functor is:
   //   double jacobians_data[6];
   //   double* jacobians_ptrs[] = {
-  //       jacobians_data + 0,
-  //       jacobians_data + 6,
-  //       jacobians_data + 8,
+  //       jacobians[0] ? jacobians[0] : jacobians_data + 0,
+  //       jacobians[1] ? jacobians[1] : jacobians_data + 6,
+  //       jacobians[2] ? jacobians[2] : jacobians_data + 8,
   //   };
-  output.emplace_back("   double jacobians_data[" +
+  output.emplace_back("  double jacobians_data[" +
                       std::to_string(kNumParameters * kNumResiduals) + "];");
-  output.emplace_back("   double* jacobians_ptrs[] = {");
+  output.emplace_back("  double* jacobians_ptrs[] = {");
   for (int i = 0, total_param_id = 0; i < kNumParameterBlocks;
        total_param_id += ParameterDims::GetDim(i), ++i) {
-    output.emplace_back("     jacobians_data + " +
+    output.emplace_back("    jacobians[" + std::to_string(i) +
+                        "] ? jacobians[" + std::to_string(i) +
+                        "] : jacobians_data + " +
                         std::to_string(kNumResiduals * total_param_id) + ",");
   }
-  output.emplace_back("   };");
-
-  // Evaluate into the tmp array.
+  output.emplace_back("  };");
   output.emplace_back(
-      "   EvaluateResidualAndJacobian(parameters, residuals, "
+      "  EvaluateResidualAndJacobian(parameters, residuals, "
       "jacobians_ptrs);");
 
-  // Copy the computed jacobians into the output array. Add an if-statement to
-  // test for null-jacobians. The generated code for a <2,3,1,2> cost functor
-  // is:
-  //    if (jacobians[0]) {
-  //      for (int i = 0; i < 6; ++i) {
-  //        jacobians[0][i] = jacobians_tmp[0][i];
-  //      }
-  //    }
-  //    if (jacobians[1]) {
-  //      for (int i = 0; i < 2; ++i) {
-  //        jacobians[1][i] = jacobians_tmp[1][i];
-  //      }
-  //    }
-  //    if (jacobians[2]) {
-  //      for (int i = 0; i < 4; ++i) {
-  //        jacobians[2][i] = jacobians_tmp[2][i];
-  //      }
-  //    }
-  for (int i = 0; i < kNumParameterBlocks; ++i) {
-    output.emplace_back("   if (jacobians[" + std::to_string(i) + "]) {");
-    output.emplace_back(
-        "     for (int i = 0; i < " +
-        std::to_string(ParameterDims::GetDim(i) * kNumResiduals) + "; ++i) {");
-    output.emplace_back("       jacobians[" + std::to_string(i) +
-                        "][i] = jacobians_ptrs[" + std::to_string(i) + "][i];");
-    output.emplace_back("     }");
-    output.emplace_back("   }");
-  }
-  output.emplace_back("   return true;");
+  output.emplace_back("  return true;");
   output.emplace_back("}");
 
   return output;