can not generate right c++ code when existing 'if ... else ...' statement

[zjhthu]

Describe the bug In my residual function, I want to return zero (invalid) reprojection error when the point is behind the camera. The residual function look like this:

    if point_cam_3d[2] > 0:
        valid_projection = T.Scalar(1)
    else:
        valid_projection = T.Scalar(0)
    return reprojection_error * valid_projection

But I find the generated C++ code can not handle this case, it returns wrong result when point_cam_3d[2] <= 0. Here are test codes: Python

from symforce import cam
from symforce import codegen
from symforce import geo
from symforce import typing as T
from symforce import sympy as sm
import os
from pathlib import Path

def exe_cmd(cmd):
    cmd = ' '.join(cmd)
    print(cmd)
    os.system(cmd)

def reprojection_error_normalized_camera(
    pose_w2c_data: geo.Vector7,
    point: geo.Vector3,
    pixel: geo.Vector2,
    epsilon: T.Scalar,
) -> geo.Vector2:

    pose_w2c = geo.Pose3.from_storage(pose_w2c_data)
    point_cam_3d = pose_w2c * point
    #'''
    if point_cam_3d[2] > 0:
        valid_projection = T.Scalar(1)
    else:
        valid_projection = T.Scalar(0)
    #'''

    point_cam_2d = geo.V2(point_cam_3d[:2]) / sm.Max(point_cam_3d[2], epsilon)
    reprojection_error = point_cam_2d - pixel
    #return reprojection_error
    return reprojection_error * valid_projection

def generate() -> None:
    output_dir = Path('./').resolve()
    reproj_codegen = codegen.Codegen.function(reprojection_error_normalized_camera, codegen.CppConfig()).with_linearization(
        which_args=["pose_w2c_data"]
    ).generate_function(output_dir=output_dir, skip_directory_nesting=True)

    print("Files generated in {}:\n".format(reproj_codegen.output_dir))
    for f in reproj_codegen.generated_files:
        print("  |- {}".format(os.path.relpath(f, reproj_codegen.output_dir)))

pose_w2c = geo.Pose3(R=geo.Rot3.random(), t=geo.Vector3([0,0,1]))
pose_w2c_data = pose_w2c.to_storage()
print('pose_w2c ' + str(pose_w2c))
print('pose_w2c_data ' + str(pose_w2c_data))

point = geo.Vector3([1,0,0])
print('point ' + str(point))

pixel=geo.Vector2([0,0])
print('pixel ' + str(pixel))

res = reprojection_error_normalized_camera(pose_w2c_data, point, pixel, epsilon=T.Scalar(1e-10))
print('res ' + str(res))

cmd = ['rm', 'reprojection_error_normalized_camera_factor.h']
exe_cmd(cmd)
generate()

cmd = ['g++', 'test_reprojection_residual.cpp', '-o', 'test_reprojection_residual', '-I', '/usr/local/include/eigen3/']
exe_cmd(cmd)

# test c++
str1 = [str(val) for val in pose_w2c_data]
str2 = [str(val) for val in point.to_storage()]
str3 = [str(val) for val in pixel.to_storage()]
cmd = ['./test_reprojection_residual'] + str1 + str2 + str3
exe_cmd(cmd)

C++

#include <iostream>
#include "reprojection_error_normalized_camera_factor.h"
#include <Eigen/Dense>

void test_factor(Eigen::Matrix<double, 7, 1>&pose_w2c, Eigen::Matrix<double, 3, 1>&point, const Eigen::Matrix<double, 2, 1>& pixel){
    Eigen::Matrix<double, 2, 1> res;
    sym::ReprojectionErrorNormalizedCameraFactor(pose_w2c, point, pixel, 1e-10, &res);
    std::cout << "res " << res << std::endl;
}

int main(int argc, char** argv){
    if (argc != 13){
        std::cerr << "reprojection_error_normalized_camera_factor pose_w2c_data point pixel" << std::endl;
        return -1;
    }

    Eigen::Matrix<double, 7, 1> pose_w2c;
    pose_w2c << std::atof(argv[1]), std::atof(argv[2]), std::atof(argv[3]),  std::atof(argv[4]), std::atof(argv[5]), std::atof(argv[6]), std::atof(argv[7]);
    std::cout << "pose_w2c " << std::endl << pose_w2c << std::endl;

    Eigen::Matrix<double, 3, 1> point;
    point << std::atof(argv[8]), std::atof(argv[9]), std::atof(argv[10]);
    std::cout << "point " << std::endl << point << std::endl;

    Eigen::Matrix<double, 2, 1> pixel;
    pixel << std::atof(argv[11]), std::atof(argv[12]);
    std::cout << "pixel " << std::endl << pixel << std::endl;

    test_factor(pose_w2c, point, pixel);
    return 0;

}

[zjhthu]

I find replacing the 'if ... else ' statement with this code can resolve this problem:

valid_projection = sm.Max(0, sm.sign(point_cam_3d[2]))

[aaron-skydio]

I'll mention that we also have sm.is_positive(point_cam_3d[2]) as a shorthand for this (and other boolean-logic-type functions you can find in symforce/logic.py)

[zjhthu]

thanks, this is usefull