y2019/vision/target_geometry.cc - RealtimeRoboticsGroup/test - Gitiles

 #include "y2019/vision/target_finder.h"

 #include "ceres/ceres.h"

 #include <math.h>

 using ceres::NumericDiffCostFunction;
 using ceres::CENTRAL;
 using ceres::CostFunction;
 using ceres::Problem;
 using ceres::Solver;
 using ceres::Solve;

 namespace y2019 {
 namespace vision {

 static constexpr double kInchesToMeters = 0.0254;

 using namespace aos::vision;
 using aos::vision::Vector;

 Target Target::MakeTemplate() {
   Target out;
   // This is how off-vertical the tape is.
   const double theta = 14.5 * M_PI / 180.0;

   const double tape_offset = 4 * kInchesToMeters;
   const double tape_width = 2 * kInchesToMeters;
   const double tape_length = 5.5 * kInchesToMeters;

   const double s = sin(theta);
   const double c = cos(theta);
   out.right.top = Vector<2>(tape_offset, 0.0);
   out.right.inside = Vector<2>(tape_offset + tape_width * c, tape_width * s);
   out.right.bottom = Vector<2>(tape_offset + tape_width * c + tape_length * s,
                                tape_width * s - tape_length * c);
   out.right.outside =
       Vector<2>(tape_offset + tape_length * s, -tape_length * c);

   out.right.is_right = true;
   out.left.top = Vector<2>(-out.right.top.x(), out.right.top.y());
   out.left.inside = Vector<2>(-out.right.inside.x(), out.right.inside.y());
   out.left.bottom = Vector<2>(-out.right.bottom.x(), out.right.bottom.y());
   out.left.outside = Vector<2>(-out.right.outside.x(), out.right.outside.y());
   return out;
 }

 std::array<Vector<2>, 8> Target::ToPointList() const {
   return std::array<Vector<2>, 8>{{right.top, right.outside, right.inside,
                                    right.bottom, left.top, left.outside,
                                    left.inside, left.bottom}};
 }

 Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
                   const ExtrinsicParams &extrinsics) {
   const double y = extrinsics.y;    // height
   const double z = extrinsics.z;    // distance
   const double r1 = extrinsics.r1;  // skew
   const double r2 = extrinsics.r2;  // heading
   const double rup = intrinsics.mount_angle;
   const double rbarrel = intrinsics.barrel_mount;
   const double fl = intrinsics.focal_length;

   // Start by translating point in target-space to be at correct height.
   ::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};

   {
     // Rotate to compensate for skew angle, to get into a frame still at the
     // same (x, y) position as the target but rotated to be facing straight
     // towards the camera.
     const double theta = r1;
     const double s = sin(theta);
     const double c = cos(theta);
     pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
            c).finished() *
           pts.transpose();
   }

   // Translate the coordinate frame to have (x, y) centered at the camera, but
   // still oriented to be facing along the line from the camera to the target.
   pts(2) += z;

   {
     // Rotate out the heading so that the frame is oriented to line up with the
     // camera's viewpoint in the yaw-axis.
     const double theta = r2;
     const double s = sin(theta);
     const double c = cos(theta);
     pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
            c).finished() *
           pts.transpose();
   }

   // TODO: Apply 15 degree downward rotation.
   {
     // Compensate for rotation in the pitch of the camera up/down to get into
     // the coordinate frame lined up with the plane of the camera sensor.
     const double theta = rup;
     const double s = sin(theta);
     const double c = cos(theta);

     pts = (::Eigen::Matrix<double, 3, 3>() << 1, 0, 0, 0, c, -s, 0, s,
            c).finished() *
           pts.transpose();
   }

   // Compensate for rotation of the barrel of the camera, i.e. about the axis
   // that points straight out from the camera lense, using an AngleAxis instead
   // of manually constructing the rotation matrices because once you get into
   // this frame you no longer need to be masochistic.
   // TODO: Maybe barrel should be extrinsics to allow rocking?
   // Also, in this case, barrel should go above the rotation above?
   pts = ::Eigen::AngleAxis<double>(rbarrel, ::Eigen::Vector3d(0.0, 0.0, 1.0)) *
         pts.transpose();

   // TODO: Final image projection.
   const ::Eigen::Matrix<double, 1, 3> res = pts;

   // Finally, scale to account for focal length and translate to get into
   // pixel-space.
   const float scale = fl / res.z();
   return Vector<2>(res.x() * scale + 320.0, 240.0 - res.y() * scale);
 }

 Target Project(const Target &target, const IntrinsicParams &intrinsics,
                const ExtrinsicParams &extrinsics) {
   Target new_targ;
   new_targ.right.is_right = true;
   new_targ.right.top = Project(target.right.top, intrinsics, extrinsics);
   new_targ.right.inside = Project(target.right.inside, intrinsics, extrinsics);
   new_targ.right.bottom = Project(target.right.bottom, intrinsics, extrinsics);
   new_targ.right.outside =
       Project(target.right.outside, intrinsics, extrinsics);

   new_targ.left.top = Project(target.left.top, intrinsics, extrinsics);
   new_targ.left.inside = Project(target.left.inside, intrinsics, extrinsics);
   new_targ.left.bottom = Project(target.left.bottom, intrinsics, extrinsics);
   new_targ.left.outside = Project(target.left.outside, intrinsics, extrinsics);

   return new_targ;
 }

 // Used at runtime on a single image given camera parameters.
 struct PointCostFunctor {
   PointCostFunctor(Vector<2> result, Vector<2> template_pt,
                      IntrinsicParams intrinsics)
       : result(result), template_pt(template_pt), intrinsics(intrinsics) {}

   bool operator()(const double *const x, double *residual) const {
     auto extrinsics = ExtrinsicParams::get(x);
     auto pt = result - Project(template_pt, intrinsics, extrinsics);
     residual[0] = pt.x();
     residual[1] = pt.y();
     return true;
   }

   Vector<2> result;
   Vector<2> template_pt;
   IntrinsicParams intrinsics;
 };

 // Find the distance from a lower target point to the 'vertical' line it should
 // be on.
 struct LineCostFunctor {
   LineCostFunctor(Vector<2> result, Segment<2> template_seg,
                   IntrinsicParams intrinsics)
       : result(result), template_seg(template_seg), intrinsics(intrinsics) {}

   bool operator()(const double *const x, double *residual) const {
     const auto extrinsics = ExtrinsicParams::get(x);
     const auto p1 = Project(template_seg.A(), intrinsics, extrinsics);
     const auto p2 = Project(template_seg.B(), intrinsics, extrinsics);
     // distance from line (P1, P2) to point result
     double dx = p2.x() - p1.x();
     double dy = p2.y() - p1.y();
     double denom = p2.DistanceTo(p1);
     residual[0] = ::std::abs(dy * result.x() - dx * result.y() +
                              p2.x() * p1.y() - p2.y() * p1.x()) /
                   denom;
     return true;
   }

   Vector<2> result;
   Segment<2> template_seg;
   IntrinsicParams intrinsics;
 };

 IntermediateResult TargetFinder::ProcessTargetToResult(const Target &target,
                                                        bool verbose) {
   // Memory for the ceres solver.
   double params_8point[ExtrinsicParams::kNumParams];
   default_extrinsics_.set(&params_8point[0]);
   double params_4point[ExtrinsicParams::kNumParams];
   default_extrinsics_.set(&params_4point[0]);

   Problem problem_8point;
   Problem problem_4point;

   ::std::array<aos::vision::Vector<2>, 8> target_value = target.ToPointList();
   ::std::array<aos::vision::Vector<2>, 8> template_value =
       target_template_.ToPointList();

   for (size_t i = 0; i < 8; ++i) {
     aos::vision::Vector<2> a = template_value[i];
     aos::vision::Vector<2> b = target_value[i];

     if (i % 2 == 1) {
       aos::vision::Vector<2> a2 = template_value[i-1];
       aos::vision::Segment<2> line = Segment<2>(a, a2);

       problem_4point.AddResidualBlock(
           new NumericDiffCostFunction<LineCostFunctor, CENTRAL, 1, 4>(
               new LineCostFunctor(b, line, intrinsics_)),
           NULL, &params_8point[0]);
     } else {
       problem_4point.AddResidualBlock(
           new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
               new PointCostFunctor(b, a, intrinsics_)),
           NULL, &params_8point[0]);
     }

     problem_8point.AddResidualBlock(
         new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
             new PointCostFunctor(b, a, intrinsics_)),
         NULL, &params_4point[0]);
   }

   Solver::Options options;
   options.minimizer_progress_to_stdout = false;
   Solver::Summary summary_8point;
   Solve(options, &problem_8point, &summary_8point);
   Solver::Summary summary_4point;
   Solve(options, &problem_4point, &summary_4point);

   IntermediateResult IR;
   IR.extrinsics = ExtrinsicParams::get(&params_8point[0]);
   IR.solver_error = summary_8point.final_cost;
   IR.backup_extrinsics = ExtrinsicParams::get(&params_4point[0]);
   IR.backup_solver_error = summary_4point.final_cost;

   if (verbose) {
     std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
     std::cout << "fl = " << intrinsics_.focal_length << ";\n";
     std::cout << "8 points:\n";
     std::cout << summary_8point.BriefReport() << "\n";
     std::cout << "error = " << summary_8point.final_cost << ";\n";
     std::cout << "y = " << IR.extrinsics.y / kInchesToMeters << ";\n";
     std::cout << "z = " << IR.extrinsics.z / kInchesToMeters << ";\n";
     std::cout << "r1 = " << IR.extrinsics.r1 * 180 / M_PI << ";\n";
     std::cout << "r2 = " << IR.extrinsics.r2 * 180 / M_PI << ";\n";
     std::cout << "4 points:\n";
     std::cout << summary_4point.BriefReport() << "\n";
     std::cout << "error = " << summary_4point.final_cost << ";\n\n";
     std::cout << "y = " << IR.backup_extrinsics.y / kInchesToMeters << ";\n";
     std::cout << "z = " << IR.backup_extrinsics.z / kInchesToMeters << ";\n";
     std::cout << "r1 = " << IR.backup_extrinsics.r1 * 180 / M_PI << ";\n";
     std::cout << "r2 = " << IR.backup_extrinsics.r2 * 180 / M_PI << ";\n";
   }
   return IR;
 }

 }  // namespace vision
 }  // namespace y2019
	#include "y2019/vision/target_finder.h"

	#include "ceres/ceres.h"

	#include <math.h>

	using ceres::NumericDiffCostFunction;
	using ceres::CENTRAL;
	using ceres::CostFunction;
	using ceres::Problem;
	using ceres::Solver;
	using ceres::Solve;

	namespace y2019 {
	namespace vision {

	static constexpr double kInchesToMeters = 0.0254;

	using namespace aos::vision;
	using aos::vision::Vector;

	Target Target::MakeTemplate() {
	Target out;
	// This is how off-vertical the tape is.
	const double theta = 14.5 * M_PI / 180.0;

	const double tape_offset = 4 * kInchesToMeters;
	const double tape_width = 2 * kInchesToMeters;
	const double tape_length = 5.5 * kInchesToMeters;

	const double s = sin(theta);
	const double c = cos(theta);
	out.right.top = Vector<2>(tape_offset, 0.0);
	out.right.inside = Vector<2>(tape_offset + tape_width * c, tape_width * s);
	out.right.bottom = Vector<2>(tape_offset + tape_width * c + tape_length * s,
	tape_width * s - tape_length * c);
	out.right.outside =
	Vector<2>(tape_offset + tape_length * s, -tape_length * c);

	out.right.is_right = true;
	out.left.top = Vector<2>(-out.right.top.x(), out.right.top.y());
	out.left.inside = Vector<2>(-out.right.inside.x(), out.right.inside.y());
	out.left.bottom = Vector<2>(-out.right.bottom.x(), out.right.bottom.y());
	out.left.outside = Vector<2>(-out.right.outside.x(), out.right.outside.y());
	return out;
	}

	std::array<Vector<2>, 8> Target::ToPointList() const {
	return std::array<Vector<2>, 8>{{right.top, right.outside, right.inside,
	right.bottom, left.top, left.outside,
	left.inside, left.bottom}};
	}

	Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
	const ExtrinsicParams &extrinsics) {
	const double y = extrinsics.y; // height
	const double z = extrinsics.z; // distance
	const double r1 = extrinsics.r1; // skew
	const double r2 = extrinsics.r2; // heading
	const double rup = intrinsics.mount_angle;
	const double rbarrel = intrinsics.barrel_mount;
	const double fl = intrinsics.focal_length;

	// Start by translating point in target-space to be at correct height.
	::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};

	{
	// Rotate to compensate for skew angle, to get into a frame still at the
	// same (x, y) position as the target but rotated to be facing straight
	// towards the camera.
	const double theta = r1;
	const double s = sin(theta);
	const double c = cos(theta);
	pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
	c).finished() *
	pts.transpose();
	}

	// Translate the coordinate frame to have (x, y) centered at the camera, but
	// still oriented to be facing along the line from the camera to the target.
	pts(2) += z;

	{
	// Rotate out the heading so that the frame is oriented to line up with the
	// camera's viewpoint in the yaw-axis.
	const double theta = r2;
	const double s = sin(theta);
	const double c = cos(theta);
	pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
	c).finished() *
	pts.transpose();
	}

	// TODO: Apply 15 degree downward rotation.
	{
	// Compensate for rotation in the pitch of the camera up/down to get into
	// the coordinate frame lined up with the plane of the camera sensor.
	const double theta = rup;
	const double s = sin(theta);
	const double c = cos(theta);

	pts = (::Eigen::Matrix<double, 3, 3>() << 1, 0, 0, 0, c, -s, 0, s,
	c).finished() *
	pts.transpose();
	}

	// Compensate for rotation of the barrel of the camera, i.e. about the axis
	// that points straight out from the camera lense, using an AngleAxis instead
	// of manually constructing the rotation matrices because once you get into
	// this frame you no longer need to be masochistic.
	// TODO: Maybe barrel should be extrinsics to allow rocking?
	// Also, in this case, barrel should go above the rotation above?
	pts = ::Eigen::AngleAxis<double>(rbarrel, ::Eigen::Vector3d(0.0, 0.0, 1.0)) *
	pts.transpose();

	// TODO: Final image projection.
	const ::Eigen::Matrix<double, 1, 3> res = pts;

	// Finally, scale to account for focal length and translate to get into
	// pixel-space.
	const float scale = fl / res.z();
	return Vector<2>(res.x() * scale + 320.0, 240.0 - res.y() * scale);
	}

	Target Project(const Target &target, const IntrinsicParams &intrinsics,
	const ExtrinsicParams &extrinsics) {
	Target new_targ;
	new_targ.right.is_right = true;
	new_targ.right.top = Project(target.right.top, intrinsics, extrinsics);
	new_targ.right.inside = Project(target.right.inside, intrinsics, extrinsics);
	new_targ.right.bottom = Project(target.right.bottom, intrinsics, extrinsics);
	new_targ.right.outside =
	Project(target.right.outside, intrinsics, extrinsics);

	new_targ.left.top = Project(target.left.top, intrinsics, extrinsics);
	new_targ.left.inside = Project(target.left.inside, intrinsics, extrinsics);
	new_targ.left.bottom = Project(target.left.bottom, intrinsics, extrinsics);
	new_targ.left.outside = Project(target.left.outside, intrinsics, extrinsics);

	return new_targ;
	}

	// Used at runtime on a single image given camera parameters.
	struct PointCostFunctor {
	PointCostFunctor(Vector<2> result, Vector<2> template_pt,
	IntrinsicParams intrinsics)
	: result(result), template_pt(template_pt), intrinsics(intrinsics) {}

	bool operator()(const double const x, double residual) const {
	auto extrinsics = ExtrinsicParams::get(x);
	auto pt = result - Project(template_pt, intrinsics, extrinsics);
	residual[0] = pt.x();
	residual[1] = pt.y();
	return true;
	}

	Vector<2> result;
	Vector<2> template_pt;
	IntrinsicParams intrinsics;
	};

	// Find the distance from a lower target point to the 'vertical' line it should
	// be on.
	struct LineCostFunctor {
	LineCostFunctor(Vector<2> result, Segment<2> template_seg,
	IntrinsicParams intrinsics)
	: result(result), template_seg(template_seg), intrinsics(intrinsics) {}

	bool operator()(const double const x, double residual) const {
	const auto extrinsics = ExtrinsicParams::get(x);
	const auto p1 = Project(template_seg.A(), intrinsics, extrinsics);
	const auto p2 = Project(template_seg.B(), intrinsics, extrinsics);
	// distance from line (P1, P2) to point result
	double dx = p2.x() - p1.x();
	double dy = p2.y() - p1.y();
	double denom = p2.DistanceTo(p1);
	residual[0] = ::std::abs(dy * result.x() - dx * result.y() +
	p2.x() * p1.y() - p2.y() * p1.x()) /
	denom;
	return true;
	}

	Vector<2> result;
	Segment<2> template_seg;
	IntrinsicParams intrinsics;
	};

	IntermediateResult TargetFinder::ProcessTargetToResult(const Target &target,
	bool verbose) {
	// Memory for the ceres solver.
	double params_8point[ExtrinsicParams::kNumParams];
	default_extrinsics_.set(&params_8point[0]);
	double params_4point[ExtrinsicParams::kNumParams];
	default_extrinsics_.set(&params_4point[0]);

	Problem problem_8point;
	Problem problem_4point;

	::std::array<aos::vision::Vector<2>, 8> target_value = target.ToPointList();
	::std::array<aos::vision::Vector<2>, 8> template_value =
	target_template_.ToPointList();

	for (size_t i = 0; i < 8; ++i) {
	aos::vision::Vector<2> a = template_value[i];
	aos::vision::Vector<2> b = target_value[i];

	if (i % 2 == 1) {
	aos::vision::Vector<2> a2 = template_value[i-1];
	aos::vision::Segment<2> line = Segment<2>(a, a2);

	problem_4point.AddResidualBlock(
	new NumericDiffCostFunction<LineCostFunctor, CENTRAL, 1, 4>(
	new LineCostFunctor(b, line, intrinsics_)),
	NULL, &params_8point[0]);
	} else {
	problem_4point.AddResidualBlock(
	new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
	new PointCostFunctor(b, a, intrinsics_)),
	NULL, &params_8point[0]);
	}

	problem_8point.AddResidualBlock(
	new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
	new PointCostFunctor(b, a, intrinsics_)),
	NULL, &params_4point[0]);
	}

	Solver::Options options;
	options.minimizer_progress_to_stdout = false;
	Solver::Summary summary_8point;
	Solve(options, &problem_8point, &summary_8point);
	Solver::Summary summary_4point;
	Solve(options, &problem_4point, &summary_4point);

	IntermediateResult IR;
	IR.extrinsics = ExtrinsicParams::get(&params_8point[0]);
	IR.solver_error = summary_8point.final_cost;
	IR.backup_extrinsics = ExtrinsicParams::get(&params_4point[0]);
	IR.backup_solver_error = summary_4point.final_cost;

	if (verbose) {
	std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
	std::cout << "fl = " << intrinsics_.focal_length << ";\n";
	std::cout << "8 points:\n";
	std::cout << summary_8point.BriefReport() << "\n";
	std::cout << "error = " << summary_8point.final_cost << ";\n";
	std::cout << "y = " << IR.extrinsics.y / kInchesToMeters << ";\n";
	std::cout << "z = " << IR.extrinsics.z / kInchesToMeters << ";\n";
	std::cout << "r1 = " << IR.extrinsics.r1 * 180 / M_PI << ";\n";
	std::cout << "r2 = " << IR.extrinsics.r2 * 180 / M_PI << ";\n";
	std::cout << "4 points:\n";
	std::cout << summary_4point.BriefReport() << "\n";
	std::cout << "error = " << summary_4point.final_cost << ";\n\n";
	std::cout << "y = " << IR.backup_extrinsics.y / kInchesToMeters << ";\n";
	std::cout << "z = " << IR.backup_extrinsics.z / kInchesToMeters << ";\n";
	std::cout << "r1 = " << IR.backup_extrinsics.r1 * 180 / M_PI << ";\n";
	std::cout << "r2 = " << IR.backup_extrinsics.r2 * 180 / M_PI << ";\n";
	}
	return IR;
	}

	} // namespace vision
	} // namespace y2019